rust/sat-rs
6
1
Fork 0
Code Issues 9 Pull Requests Projects Releases 24 Wiki Activity

Mode Tree Feature Update

Browse Source
This commit is contained in:
Robin Müller
2024-11-21 18:35:16 +01:00
parent 1a1d330814
commit 518028c8fc
23 changed files with 5000 additions and 671 deletions
Download Patch File Download Diff File Expand all files Collapse all files

165
satrs-example/src/acs/mgm.rs
View File

@ -1,7 +1,7 @@
use derive_new::new;
use satrs::hk::{HkRequest, HkRequestVariant};
use satrs::mode_tree::{ModeChild, ModeNode};
use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender};
use satrs::queue::{GenericSendError, GenericTargetedMessagingError};
use satrs_example::{DeviceMode, TimestampHelper};
use satrs_minisim::acs::lis3mdl::{
MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR,
@ -16,6 +16,7 @@ use std::time::Duration;
use satrs::mode::{
ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler,
ModeRequestHandlerMpscBounded,
};
use satrs::pus::{EcssTmSender, PusTmVariant};
use satrs::request::{GenericMessage, MessageMetadata, UniqueApidTargetId};
@ -171,7 +172,7 @@ pub struct MgmHandlerLis3Mdl<
> {
id: UniqueApidTargetId,
dev_str: &'static str,
mode_interface: MpscModeLeafInterface,
mode_node: ModeRequestHandlerMpscBounded,
composite_request_rx: mpsc::Receiver<GenericMessage<CompositeRequest>>,
hk_reply_tx: mpsc::Sender<GenericMessage<HkReply>>,
switch_helper: SwitchHelper,
@ -275,25 +276,28 @@ impl<
pub fn handle_mode_requests(&mut self) {
loop {
// TODO: Only allow one set mode request per cycle?
match self.mode_interface.request_rx.try_recv() {
Ok(msg) => {
let result = self.handle_mode_request(msg);
// TODO: Trigger event?
if result.is_err() {
log::warn!(
"{}: mode request failed with error {:?}",
self.dev_str,
result.err().unwrap()
);
}
}
Err(e) => {
if e != mpsc::TryRecvError::Empty {
log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e);
match self.mode_node.try_recv_mode_request() {
Ok(opt_msg) => {
if let Some(msg) = opt_msg {
let result = self.handle_mode_request(msg);
// TODO: Trigger event?
if result.is_err() {
log::warn!(
"{}: mode request failed with error {:?}",
self.dev_str,
result.err().unwrap()
);
}
} else {
break;
}
}
Err(e) => match e {
satrs::queue::GenericReceiveError::Empty => break,
satrs::queue::GenericReceiveError::TxDisconnected(e) => {
log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e);
}
},
}
}
}
@ -386,6 +390,7 @@ impl<
&mut self,
requestor: MessageMetadata,
mode_and_submode: ModeAndSubmode,
_forced: bool,
) -> Result<(), satrs::mode::ModeError> {
log::info!(
"{}: transitioning to mode {:?}",
@ -448,10 +453,9 @@ impl<
requestor.sender_id()
);
}
self.mode_interface
.reply_to_pus_tx
.send(GenericMessage::new(requestor, reply))
.map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
self.mode_node
.send_mode_reply(requestor, reply)
.map_err(ModeError::Send)?;
Ok(())
}
@ -464,17 +468,46 @@ impl<
}
}
impl<
ComInterface: SpiInterface,
TmSender: EcssTmSender,
SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
> ModeNode for MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
{
fn id(&self) -> satrs::ComponentId {
self.id.into()
}
}
impl<
ComInterface: SpiInterface,
TmSender: EcssTmSender,
SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
> ModeChild for MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
{
type Sender = mpsc::SyncSender<GenericMessage<ModeReply>>;
fn add_mode_parent(&mut self, id: satrs::ComponentId, reply_sender: Self::Sender) {
self.mode_node.add_message_target(id, reply_sender);
}
}
#[cfg(test)]
mod tests {
use std::sync::{mpsc, Arc};
use std::{
collections::HashMap,
sync::{mpsc, Arc},
};
use satrs::{
mode::{ModeReply, ModeRequest},
mode_tree::ModeParent,
power::SwitchStateBinary,
request::{GenericMessage, UniqueApidTargetId},
tmtc::PacketAsVec,
ComponentId,
};
use satrs_example::config::components::Apid;
use satrs_example::config::components::{Apid, MGM_ASSEMBLY};
use satrs_minisim::acs::lis3mdl::MgmLis3RawValues;
use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest};
@ -503,7 +536,7 @@ mod tests {
}
pub struct MgmTestbench {
pub mode_request_tx: mpsc::Sender<GenericMessage<ModeRequest>>,
pub mode_request_tx: mpsc::SyncSender<GenericMessage<ModeRequest>>,
pub mode_reply_rx_to_pus: mpsc::Receiver<GenericMessage<ModeReply>>,
pub mode_reply_rx_to_parent: mpsc::Receiver<GenericMessage<ModeReply>>,
pub composite_request_tx: mpsc::Sender<GenericMessage<CompositeRequest>>,
@ -513,38 +546,76 @@ mod tests {
MgmHandlerLis3Mdl<TestSpiInterface, mpsc::Sender<PacketAsVec>, TestSwitchHelper>,
}
#[derive(Default)]
pub struct MgmAssemblyMock(
pub HashMap<ComponentId, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
);
impl ModeNode for MgmAssemblyMock {
fn id(&self) -> satrs::ComponentId {
PUS_MODE_SERVICE.into()
}
}
impl ModeParent for MgmAssemblyMock {
type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;
fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) {
self.0.insert(id, request_sender);
}
}
#[derive(Default)]
pub struct PusMock {
pub request_sender_map: HashMap<ComponentId, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
}
impl ModeNode for PusMock {
fn id(&self) -> satrs::ComponentId {
PUS_MODE_SERVICE.into()
}
}
impl ModeParent for PusMock {
type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;
fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) {
self.request_sender_map.insert(id, request_sender);
}
}
impl MgmTestbench {
pub fn new() -> Self {
let (request_tx, request_rx) = mpsc::channel();
let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel();
let (request_tx, request_rx) = mpsc::sync_channel(5);
let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::sync_channel(5);
let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5);
let mode_interface = MpscModeLeafInterface {
request_rx,
reply_to_pus_tx: reply_tx_to_pus,
reply_to_parent_tx: reply_tx_to_parent,
};
let id = UniqueApidTargetId::new(Apid::Acs as u16, 1);
let mode_node = ModeRequestHandlerMpscBounded::new(id.into(), request_rx);
let (composite_request_tx, composite_request_rx) = mpsc::channel();
let (hk_reply_tx, hk_reply_rx) = mpsc::channel();
let (tm_tx, tm_rx) = mpsc::channel::<PacketAsVec>();
let shared_mgm_set = Arc::default();
let mut handler = MgmHandlerLis3Mdl::new(
id,
"TEST_MGM",
mode_node,
composite_request_rx,
hk_reply_tx,
TestSwitchHelper::default(),
tm_tx,
TestSpiInterface::default(),
shared_mgm_set,
);
handler.add_mode_parent(PUS_MODE_SERVICE.into(), reply_tx_to_pus);
handler.add_mode_parent(MGM_ASSEMBLY.into(), reply_tx_to_parent);
Self {
mode_request_tx: request_tx,
mode_reply_rx_to_pus: reply_rx_to_pus,
mode_reply_rx_to_parent: reply_rx_to_parent,
composite_request_tx,
handler,
tm_rx,
hk_reply_rx,
handler: MgmHandlerLis3Mdl::new(
UniqueApidTargetId::new(Apid::Acs as u16, 1),
"TEST_MGM",
mode_interface,
composite_request_rx,
hk_reply_tx,
TestSwitchHelper::default(),
tm_tx,
TestSpiInterface::default(),
shared_mgm_set,
),
}
}
}
@ -575,7 +646,10 @@ mod tests {
.mode_request_tx
.send(GenericMessage::new(
MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
ModeRequest::SetMode {
mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
forced: false,
},
))
.expect("failed to send mode request");
testbench.handler.periodic_operation();
@ -633,7 +707,10 @@ mod tests {
.mode_request_tx
.send(GenericMessage::new(
MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
ModeRequest::SetMode {
mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
forced: false,
},
))
.expect("failed to send mode request");
testbench.handler.periodic_operation();

3
satrs-example/src/config.rs
View File

@ -149,6 +149,7 @@ pub mod components {
#[derive(Copy, Clone, PartialEq, Eq)]
pub enum AcsId {
Mgm0 = 0,
Assembly = 1,
}
#[derive(Copy, Clone, PartialEq, Eq)]
@ -176,6 +177,8 @@ pub mod components {
UniqueApidTargetId::new(Apid::GenericPus as u16, PusId::PusHk as u32);
pub const PUS_SCHED_SERVICE: UniqueApidTargetId =
UniqueApidTargetId::new(Apid::Sched as u16, 0);
pub const MGM_ASSEMBLY: UniqueApidTargetId =
UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Assembly as u32);
pub const MGM_HANDLER_0: UniqueApidTargetId =
UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Mgm0 as u32);
pub const PCDU_HANDLER: UniqueApidTargetId =

15
satrs-example/src/eps/pcdu.rs
View File

@ -11,7 +11,7 @@ use satrs::{
mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler},
power::SwitchRequest,
pus::{EcssTmSender, PusTmVariant},
queue::{GenericSendError, GenericTargetedMessagingError},
queue::GenericSendError,
request::{GenericMessage, MessageMetadata, UniqueApidTargetId},
spacepackets::ByteConversionError,
};
@ -412,6 +412,7 @@ impl<ComInterface: SerialInterface, TmSender: EcssTmSender> ModeRequestHandler
&mut self,
requestor: MessageMetadata,
mode_and_submode: ModeAndSubmode,
_forced: bool,
) -> Result<(), satrs::mode::ModeError> {
log::info!(
"{}: transitioning to mode {:?}",
@ -469,7 +470,7 @@ impl<ComInterface: SerialInterface, TmSender: EcssTmSender> ModeRequestHandler
self.mode_interface
.reply_to_pus_tx
.send(GenericMessage::new(requestor, reply))
.map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
.map_err(|_| GenericSendError::RxDisconnected)?;
Ok(())
}
@ -660,7 +661,10 @@ mod tests {
.mode_request_tx
.send(GenericMessage::new(
MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
ModeRequest::SetMode {
mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
forced: false,
},
))
.expect("failed to send mode request");
let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap();
@ -692,7 +696,10 @@ mod tests {
.mode_request_tx
.send(GenericMessage::new(
MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
ModeRequest::SetMode {
mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
forced: false,
},
))
.expect("failed to send mode request");
testbench

54
satrs-example/src/main.rs
View File

@ -21,6 +21,7 @@ use log::info;
use pus::test::create_test_service_dynamic;
use satrs::hal::std::tcp_server::ServerConfig;
use satrs::hal::std::udp_server::UdpTcServer;
use satrs::mode_tree::connect_mode_nodes;
use satrs::pus::HandlingStatus;
use satrs::request::{GenericMessage, MessageMetadata};
use satrs::tmtc::{PacketSenderWithSharedPool, SharedPacketPool};
@ -47,7 +48,7 @@ use crate::pus::scheduler::{create_scheduler_service_dynamic, create_scheduler_s
use crate::pus::test::create_test_service_static;
use crate::pus::{PusTcDistributor, PusTcMpscRouter};
use crate::requests::{CompositeRequest, GenericRequestRouter};
use satrs::mode::{Mode, ModeAndSubmode, ModeRequest};
use satrs::mode::{Mode, ModeAndSubmode, ModeRequest, ModeRequestHandlerMpscBounded};
use satrs::pus::event_man::EventRequestWithToken;
use satrs::spacepackets::{time::cds::CdsTime, time::TimeWriter};
use satrs_example::config::components::{
@ -93,9 +94,6 @@ fn static_tmtc_pool_main() {
request_map
.composite_router_map
.insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx);
request_map
.mode_router_map
.insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx);
request_map
.composite_router_map
.insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx);
@ -232,12 +230,8 @@ fn static_tmtc_pool_main() {
let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone());
let shared_mgm_set = Arc::default();
let mgm_mode_leaf_interface = MpscModeLeafInterface {
request_rx: mgm_handler_mode_rx,
reply_to_pus_tx: pus_mode_reply_tx.clone(),
reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
};
let mgm_mode_node =
ModeRequestHandlerMpscBounded::new(MGM_HANDLER_0.into(), mgm_handler_mode_rx);
let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx);
SpiSimInterfaceWrapper::Sim(SpiSimInterface {
@ -250,7 +244,7 @@ fn static_tmtc_pool_main() {
let mut mgm_handler = MgmHandlerLis3Mdl::new(
MGM_HANDLER_0,
"MGM_0",
mgm_mode_leaf_interface,
mgm_mode_node,
mgm_handler_composite_rx,
pus_hk_reply_tx.clone(),
switch_helper.clone(),
@ -258,6 +252,13 @@ fn static_tmtc_pool_main() {
mgm_spi_interface,
shared_mgm_set,
);
// Connect PUS mode service to device handler.
connect_mode_nodes(
&mut pus_stack.mode_srv,
mgm_handler_mode_tx,
&mut mgm_handler,
mgm_handler_mode_reply_to_parent_tx,
);
let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) =
mpsc::sync_channel(10);
@ -291,7 +292,10 @@ fn static_tmtc_pool_main() {
pcdu_handler_mode_tx
.send(GenericMessage::new(
MessageMetadata::new(0, NO_SENDER),
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)),
ModeRequest::SetMode {
mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0),
forced: false,
},
))
.expect("sending initial mode request failed");
@ -423,9 +427,9 @@ fn dyn_tmtc_pool_main() {
request_map
.composite_router_map
.insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx);
request_map
.mode_router_map
.insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx);
//request_map
//.mode_router_map
//.insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx);
request_map
.composite_router_map
.insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx);
@ -540,11 +544,8 @@ fn dyn_tmtc_pool_main() {
let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) =
mpsc::sync_channel(5);
let shared_mgm_set = Arc::default();
let mode_leaf_interface = MpscModeLeafInterface {
request_rx: mgm_handler_mode_rx,
reply_to_pus_tx: pus_mode_reply_tx.clone(),
reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
};
let mgm_mode_node =
ModeRequestHandlerMpscBounded::new(MGM_HANDLER_0.into(), mgm_handler_mode_rx);
let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx);
@ -558,7 +559,7 @@ fn dyn_tmtc_pool_main() {
let mut mgm_handler = MgmHandlerLis3Mdl::new(
MGM_HANDLER_0,
"MGM_0",
mode_leaf_interface,
mgm_mode_node,
mgm_handler_composite_rx,
pus_hk_reply_tx.clone(),
switch_helper.clone(),
@ -566,6 +567,12 @@ fn dyn_tmtc_pool_main() {
mgm_spi_interface,
shared_mgm_set,
);
connect_mode_nodes(
&mut pus_stack.mode_srv,
mgm_handler_mode_tx,
&mut mgm_handler,
mgm_handler_mode_reply_to_parent_tx,
);
let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) =
mpsc::sync_channel(10);
@ -598,7 +605,10 @@ fn dyn_tmtc_pool_main() {
pcdu_handler_mode_tx
.send(GenericMessage::new(
MessageMetadata::new(0, NO_SENDER),
ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)),
ModeRequest::SetMode {
mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0),
forced: false,
},
))
.expect("sending initial mode request failed");

39
satrs-example/src/pus/mode.rs
View File

@ -1,4 +1,5 @@
use derive_new::new;
use satrs::mode_tree::{ModeNode, ModeParent};
use satrs::tmtc::{PacketAsVec, PacketSenderWithSharedPool};
use std::sync::mpsc;
use std::time::Duration;
@ -110,6 +111,7 @@ impl PusReplyHandler<ActivePusRequestStd, ModeReply> for ModeReplyHandler {
),
)?;
}
ModeReply::ModeInfo(_mode_and_submode) => (),
};
Ok(true)
}
@ -190,7 +192,13 @@ impl PusTcToRequestConverter<ActivePusRequestStd, ModeRequest> for ModeRequestCo
}
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)))
Ok((
active_request,
ModeRequest::SetMode {
mode_and_submode,
forced: false,
},
))
}
Subservice::TcReadMode => Ok((active_request, ModeRequest::ReadMode)),
Subservice::TcAnnounceMode => Ok((active_request, ModeRequest::AnnounceMode)),
@ -268,6 +276,27 @@ pub struct ModeServiceWrapper<TmSender: EcssTmSender, TcInMemConverter: EcssTcIn
>,
}
impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> ModeNode
for ModeServiceWrapper<TmSender, TcInMemConverter>
{
fn id(&self) -> ComponentId {
self.service.service_helper.id()
}
}
impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> ModeParent
for ModeServiceWrapper<TmSender, TcInMemConverter>
{
type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;
fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) {
self.service
.request_router
.mode_router_map
.insert(id, request_sender);
}
}
impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> TargetedPusService
for ModeServiceWrapper<TmSender, TcInMemConverter>
{
@ -346,7 +375,13 @@ mod tests {
let (_active_req, req) = testbench
.convert(token, &[], TEST_APID, TEST_UNIQUE_ID_0)
.expect("conversion has failed");
assert_eq!(req, ModeRequest::SetMode(mode_and_submode));
assert_eq!(
req,
ModeRequest::SetMode {
mode_and_submode,
forced: false
}
);
}
#[test]

12
satrs-example/src/pus/stack.rs
View File

@ -15,12 +15,12 @@ use super::{
// for targeted services..
#[derive(new)]
pub struct PusStack<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> {
test_srv: TestCustomServiceWrapper<TmSender, TcInMemConverter>,
hk_srv_wrapper: HkServiceWrapper<TmSender, TcInMemConverter>,
event_srv: EventServiceWrapper<TmSender, TcInMemConverter>,
action_srv_wrapper: ActionServiceWrapper<TmSender, TcInMemConverter>,
schedule_srv: SchedulingServiceWrapper<TmSender, TcInMemConverter>,
mode_srv: ModeServiceWrapper<TmSender, TcInMemConverter>,
pub test_srv: TestCustomServiceWrapper<TmSender, TcInMemConverter>,
pub hk_srv_wrapper: HkServiceWrapper<TmSender, TcInMemConverter>,
pub event_srv: EventServiceWrapper<TmSender, TcInMemConverter>,
pub action_srv_wrapper: ActionServiceWrapper<TmSender, TcInMemConverter>,
pub schedule_srv: SchedulingServiceWrapper<TmSender, TcInMemConverter>,
pub mode_srv: ModeServiceWrapper<TmSender, TcInMemConverter>,
}
impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter>