diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs index ea9f1d4..f557e08 100644 --- a/satrs-example/src/config.rs +++ b/satrs-example/src/config.rs @@ -29,6 +29,7 @@ pub const AOCS_APID: u16 = 1; pub enum GroupId { Tmtc = 0, Hk = 1, + Mode = 2, } pub const OBSW_SERVER_ADDR: Ipv4Addr = Ipv4Addr::UNSPECIFIED; @@ -94,6 +95,13 @@ pub mod hk_err { ]; } +pub mod mode_err { + use super::*; + + #[resultcode] + pub const WRONG_MODE: ResultU16 = ResultU16::new(GroupId::Mode as u8, 0); +} + #[derive(Copy, Clone, PartialEq, Eq)] pub enum ComponentIdList { PusVerification = 0, diff --git a/satrs-example/src/pus/mod.rs b/satrs-example/src/pus/mod.rs index 2e0a368..22304c1 100644 --- a/satrs-example/src/pus/mod.rs +++ b/satrs-example/src/pus/mod.rs @@ -25,6 +25,7 @@ use std::sync::mpsc::{self, Sender}; pub mod action; pub mod event; pub mod hk; +pub mod mode; pub mod scheduler; pub mod stack; pub mod test; diff --git a/satrs-example/src/pus/mode.rs b/satrs-example/src/pus/mode.rs index 09306e6..7973c7a 100644 --- a/satrs-example/src/pus/mode.rs +++ b/satrs-example/src/pus/mode.rs @@ -1 +1,197 @@ -// TODO: Add PUS mode service, similar to action and HK service. +use std::time::Duration; + +use satrs::{ + mode::{ModeAndSubmode, ModeReply, ModeRequest}, + pus::{ + mode::Subservice, + verification::{ + self, FailParams, TcStateAccepted, TcStateStarted, VerificationReportingProvider, + VerificationToken, + }, + ActivePusRequestStd, ActiveRequestProvider, EcssTmSenderCore, EcssTmtcError, + GenericConversionError, PusReplyHandler, PusTcToRequestConverter, PusTmWrapper, + }, + request::TargetAndApidId, + spacepackets::{ + ecss::{ + tc::PusTcReader, + tm::{PusTmCreator, PusTmSecondaryHeader}, + PusPacket, + }, + SpHeader, + }, +}; +use satrs_example::config::{mode_err, tmtc_err}; + +use super::generic_pus_request_timeout_handler; + +#[derive(Default)] +pub struct ModeReplyHandler {} + +impl PusReplyHandler for ModeReplyHandler { + type Error = EcssTmtcError; + + fn handle_unexpected_reply( + &mut self, + reply: &satrs::request::GenericMessage, + _tm_sender: &impl EcssTmSenderCore, + ) -> Result<(), Self::Error> { + log::warn!("received unexpected reply for mode service 5: {reply:?}"); + Ok(()) + } + + fn handle_reply( + &mut self, + reply: &satrs::request::GenericMessage, + active_request: &ActivePusRequestStd, + verification_handler: &impl VerificationReportingProvider, + time_stamp: &[u8], + tm_sender: &impl EcssTmSenderCore, + ) -> Result { + let started_token: VerificationToken = active_request + .token() + .try_into() + .expect("invalid token state"); + match reply.message { + ModeReply::ModeInfo(info) => { + log::warn!( + "received unrequest mode information for active request {:?}: {:?}", + active_request, + info + ); + } + ModeReply::ModeReply(mode_reply) => { + let mut source_data: [u8; 12] = [0; 12]; + mode_reply + .write_to_be_bytes(&mut source_data) + .expect("writing mode reply failed"); + let req_id = verification::RequestId::from(reply.request_id); + let mut sp_header = SpHeader::tm_unseg(req_id.packet_id().apid(), 0, 0) + .expect("generating SP header failed"); + let sec_header = PusTmSecondaryHeader::new( + 200, + Subservice::TmModeReply as u8, + 0, + 0, + Some(time_stamp), + ); + let pus_tm = PusTmCreator::new(&mut sp_header, sec_header, &source_data, true); + tm_sender.send_tm(PusTmWrapper::Direct(pus_tm))?; + verification_handler + .completion_success(started_token, time_stamp) + .map_err(|e| e.0)?; + } + ModeReply::CantReachMode(error_code) => { + verification_handler + .completion_failure( + started_token, + FailParams::new(time_stamp, &error_code, &[]), + ) + .map_err(|e| e.0)?; + } + ModeReply::WrongMode { expected, reached } => { + let mut error_info: [u8; 24] = [0; 24]; + let mut written_len = expected + .write_to_be_bytes(&mut error_info[0..ModeAndSubmode::RAW_LEN]) + .expect("writing expected mode failed"); + written_len += reached + .write_to_be_bytes(&mut error_info[ModeAndSubmode::RAW_LEN..]) + .expect("writing reached mode failed"); + verification_handler + .completion_failure( + started_token, + FailParams::new( + time_stamp, + &mode_err::WRONG_MODE, + &error_info[..written_len], + ), + ) + .map_err(|e| e.0)?; + } + }; + Ok(true) + } + + fn handle_request_timeout( + &mut self, + active_request: &ActivePusRequestStd, + verification_handler: &impl VerificationReportingProvider, + time_stamp: &[u8], + _tm_sender: &impl EcssTmSenderCore, + ) -> Result<(), Self::Error> { + generic_pus_request_timeout_handler( + active_request, + verification_handler, + time_stamp, + "HK", + )?; + Ok(()) + } +} + +#[derive(Default)] +pub struct ExampleModeRequestConverter {} + +impl PusTcToRequestConverter for ExampleModeRequestConverter { + type Error = GenericConversionError; + + fn convert( + &mut self, + token: VerificationToken, + tc: &PusTcReader, + time_stamp: &[u8], + verif_reporter: &impl VerificationReportingProvider, + ) -> Result<(ActivePusRequestStd, ModeRequest), Self::Error> { + let subservice = tc.subservice(); + let user_data = tc.user_data(); + let not_enough_app_data = |expected: usize| { + verif_reporter + .start_failure( + token, + FailParams::new_no_fail_data(time_stamp, &tmtc_err::NOT_ENOUGH_APP_DATA), + ) + .expect("Sending start failure failed"); + Err(GenericConversionError::NotEnoughAppData { + expected, + found: user_data.len(), + }) + }; + if user_data.len() < core::mem::size_of::() { + return not_enough_app_data(4); + } + let target_id_and_apid = TargetAndApidId::from_pus_tc(tc).unwrap(); + let active_request = + ActivePusRequestStd::new(target_id_and_apid.into(), token, Duration::from_secs(30)); + let subservice_typed = Subservice::try_from(subservice); + let invalid_subservice = || { + // Invalid subservice + verif_reporter + .start_failure( + token, + FailParams::new_no_fail_data(time_stamp, &tmtc_err::INVALID_PUS_SUBSERVICE), + ) + .expect("Sending start failure failed"); + Err(GenericConversionError::InvalidSubservice(subservice)) + }; + if subservice_typed.is_err() { + return invalid_subservice(); + } + let subservice_typed = subservice_typed.unwrap(); + match subservice_typed { + Subservice::TcSetMode => { + if user_data.len() < core::mem::size_of::() + ModeAndSubmode::RAW_LEN { + return not_enough_app_data(4 + ModeAndSubmode::RAW_LEN); + } + let mode_and_submode = ModeAndSubmode::from_be_bytes(&tc.user_data()[4..]) + .expect("mode and submode extraction failed"); + Ok((active_request, ModeRequest::SetMode(mode_and_submode))) + } + Subservice::TcReadMode => Ok((active_request, ModeRequest::ReadMode)), + Subservice::TcAnnounceMode => Ok((active_request, ModeRequest::AnnounceMode)), + Subservice::TcAnnounceModeRecursive => { + Ok((active_request, ModeRequest::AnnounceModeRecursive)) + } + _ => invalid_subservice(), + } + } +} diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs index 7e6f1da..29335b1 100644 --- a/satrs/src/mode.rs +++ b/satrs/src/mode.rs @@ -27,7 +27,7 @@ pub struct ModeAndSubmode { } impl ModeAndSubmode { - const RAW_LEN: usize = size_of::() + size_of::(); + pub const RAW_LEN: usize = size_of::() + size_of::(); pub const fn new_mode_only(mode: Mode) -> Self { Self { mode, submode: 0 } @@ -131,6 +131,7 @@ pub enum ModeReply { ModeReply(ModeAndSubmode), // Can not reach the commanded mode. Contains a reason as a [ResultU16]. CantReachMode(ResultU16), + /// We are in the wrong mode for unknown reasons. Contains the expected and reached mode. WrongMode { expected: ModeAndSubmode, reached: ModeAndSubmode,