rust/sat-rs
12
1
Fork 0
Code Issues 6 Pull Requests 2 Projects Releases 29 Wiki Activity

try this

Browse Source
This commit is contained in:
Robin Mueller
2026-07-13 14:36:43 +02:00
parent 544a147223
commit 91c295d781
2 changed files with 166 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
satrs/src/mode_tree.rs
+12
View File
@@ -167,6 +167,10 @@ impl ModeStoreValue {
}
}
pub trait SequenceTableProvider {
fn sequence_at_index(&self, index: usize) -> Option<&[SequenceTableEntry]>;
}
#[derive(Debug, thiserror::Error)]
#[error("mode store is full")]
pub struct FullError;
@@ -396,6 +400,14 @@ pub mod alloc_mod {
}
}
impl SequenceTableProvider for SequenceTablesMapValue {
fn sequence_at_index(&self, index: usize) -> Option<&[SequenceTableEntry]> {
self.entries
.get(index)
.map(|table| table.entries.as_slice())
}
}
#[derive(Debug, Default)]
pub struct TargetModeTables(pub HashMap<ModeRaw, TargetTablesMapValue>);
satrs/src/subsystem.rs
+154 -107
View File
@@ -2,9 +2,9 @@ use crate::{
ComponentId,
mode_tree::{
ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry,
SequenceTablesMapValue,
SequenceTableProvider, SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError,
TargetTablesMapValue,
},
queue::GenericTargetedMessagingError,
request::RequestId,
};
@@ -44,6 +44,10 @@ pub enum StartSequenceError {
InvalidRequestId(RequestId),
}
#[derive(Debug, thiserror::Error)]
#[error("invalid sequence index")]
pub struct InvalidSequenceIndexError;
/// This sequence execution helper includes some boilerplate logic to
/// execute [SequenceModeTables].
///
@@ -126,10 +130,10 @@ impl SequenceExecutionHelper {
/// children components
pub fn run<F: FnMut(ComponentId, ModeRaw)>(
&mut self,
table: &SequenceModeTables,
children_mode_store: &mut ModeStoreVec,
sequence_table_provider: &impl SequenceTableProvider,
children_mode_store: &mut impl ModeStoreProvider,
mode_request_handler: F,
) -> Result<ModeCommandingResult, GenericTargetedMessagingError> {
) -> Result<ModeCommandingResult, InvalidSequenceIndexError> {
if self.state == SequenceExecutionHelperState::Idle {
return Ok(ModeCommandingResult::Done);
}
@@ -140,30 +144,31 @@ impl SequenceExecutionHelper {
return Ok(ModeCommandingResult::Done);
}
match self.current_sequence_index {
Some(idx) => {
Some(index) => {
// Execute the sequence.
let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap();
self.execute_sequence_and_map_to_result(
seq_table_value,
idx,
Ok(self.execute_sequence_and_map_to_result(
index,
sequence_table_provider.sequence_at_index(index).unwrap(),
children_mode_store,
mode_request_handler,
)
))
}
None => {
let sequence_index = 0;
let sequence = sequence_table_provider
.sequence_at_index(sequence_index)
.unwrap();
// Find the first sequence
let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap();
self.last_sequence_index = Some(seq_table_value.entries.len() - 1);
if seq_table_value.entries.is_empty() {
if sequence.is_empty() {
Ok(ModeCommandingResult::Done)
} else {
self.current_sequence_index = Some(0);
self.execute_sequence_and_map_to_result(
seq_table_value,
0,
Ok(self.execute_sequence_and_map_to_result(
sequence_index,
sequence,
children_mode_store,
mode_request_handler,
)
))
}
}
}
@@ -210,33 +215,32 @@ impl SequenceExecutionHelper {
self.current_sequence_index
}
/// Execute a sequence at the given sequence index for a given [SequenceTablesMapValue].
/// Execute a sequence at the given sequence index for a given sequence.
///
/// The sequence is identifier by a sequence index. It is represented by a list of
/// [SequenceTableEntry] values.
///
/// This method calls [Self::execute_sequence] and maps the result to a [ModeCommandingResult].
/// It is also called by the [Self::run] method of this helper.
pub fn execute_sequence_and_map_to_result(
&mut self,
seq_table_value: &SequenceTablesMapValue,
sequence_idx: usize,
sequence_index: usize,
table_entries: &[SequenceTableEntry],
children_mode_store: &mut impl ModeStoreProvider,
mode_request_handler: impl FnMut(ComponentId, ModeRaw),
) -> Result<ModeCommandingResult, GenericTargetedMessagingError> {
) -> ModeCommandingResult {
if self.state() == SequenceExecutionHelperState::Idle || self.request_id.is_none() {
return Ok(ModeCommandingResult::Done);
return ModeCommandingResult::Done;
}
if Self::execute_sequence(
&seq_table_value.entries[sequence_idx].entries,
children_mode_store,
mode_request_handler,
)? {
if Self::execute_sequence(table_entries, children_mode_store, mode_request_handler) {
self.state = SequenceExecutionHelperState::AwaitingSuccessCheck;
Ok(ModeCommandingResult::AwaitingSuccessCheck)
} else if seq_table_value.entries.len() - 1 == sequence_idx {
ModeCommandingResult::AwaitingSuccessCheck
} else if table_entries.len() - 1 == sequence_index {
self.state = SequenceExecutionHelperState::Idle;
Ok(ModeCommandingResult::Done)
ModeCommandingResult::Done
} else {
self.current_sequence_index = Some(sequence_idx + 1);
Ok(ModeCommandingResult::StepDone)
self.current_sequence_index = Some(sequence_index + 1);
ModeCommandingResult::StepDone
}
}
@@ -255,7 +259,7 @@ impl SequenceExecutionHelper {
table_entries: &[SequenceTableEntry],
children_mode_store: &mut impl ModeStoreProvider,
mut mode_request_handler: impl FnMut(ComponentId, ModeRaw),
) -> Result<bool, GenericTargetedMessagingError> {
) -> bool {
let mut some_succes_check_required = false;
for entry in table_entries {
mode_request_handler(entry.common.target_id, entry.common.mode);
@@ -264,7 +268,7 @@ impl SequenceExecutionHelper {
some_succes_check_required = true;
}
}
Ok(some_succes_check_required)
some_succes_check_required
}
}
@@ -296,10 +300,12 @@ impl From<ModeCommandingResult> for SubsystemHelperResult {
#[derive(Debug, thiserror::Error)]
pub enum ModeTreeHelperError {
#[error("generic targeted messaging error: {0}")]
Message(#[from] GenericTargetedMessagingError),
#[error("current mode {0} is not contained in target table")]
CurrentModeNotInTargetTable(ModeRaw),
#[error("No sequence table found for mode {0:?}")]
NoSequenceTableFound(Option<ModeRaw>),
#[error("invalid sequence index")]
InvalidSequenceIndex(#[from] InvalidSequenceIndexError),
/// Mode command has failed, for example while executing a mode table.
#[error("mode command failed")]
ModeCommmandFailure {
@@ -319,7 +325,6 @@ pub enum ModeTreeHelperError {
///
/// This helper object tries to compose as much data and state information as possible which is
/// required for this process.
/*
pub struct SubsystemCommandingHelper {
/// State of the helper.
state: ModeTreeHelperState,
@@ -331,12 +336,12 @@ pub struct SubsystemCommandingHelper {
/// replies are relevant for reply awaition logic.
active_internal_request_id: Option<RequestId>,
/// The primary data structure to keep the target state information for subsystem
/// [modes][Mode]. it specifies the mode each child should have for a certain subsystem mode
/// [modes][ModeRaw]. it specifies the mode each child should have for a certain subsystem mode
/// and is relevant for target keeping.
pub target_tables: TargetModeTables,
/// The primary data structure to keep the sequence commanding information for commanded
/// subsystem [modes][Mode]. It specifies the actual commands and the order they should be
/// sent in to reach a certain [mode][Mode].
/// subsystem [modes][ModeRaw]. It specifies the actual commands and the order they should be
/// sent in to reach a certain [mode][ModeRaw].
pub sequence_tables: SequenceModeTables,
/// The sequence execution helper is used to execute sequences in the [Self::sequence_tables].
pub seq_exec_helper: SequenceExecutionHelper,
@@ -356,6 +361,14 @@ impl Default for SubsystemCommandingHelper {
}
}
#[derive(Debug)]
pub struct ModeResponse {
pub request_id: RequestId,
pub sender_id: ComponentId,
pub reported_mode: ModeRaw,
pub success: bool,
}
impl SubsystemCommandingHelper {
/// Create a new substem commanding helper with an intial [ModeTreeHelperState::Idle] state,
/// an empty mode children store and empty target and sequence mode tables.
@@ -415,7 +428,8 @@ impl SubsystemCommandingHelper {
/// Add a mode child to the internal [Self::children_mode_store].
pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeRaw) {
self.children_mode_store.add_component(child, mode);
// Can not fail for regular vector.
self.children_mode_store.add_component(child, mode).unwrap();
}
/// Add a target mode table and an associated sequence mode table.
@@ -455,14 +469,13 @@ impl SubsystemCommandingHelper {
Ok(())
}
pub fn state_machine(
pub fn state_machine<F: FnMut(ComponentId, ModeRaw)>(
&mut self,
opt_reply: Option<GenericMessage<ModeReply>>,
req_sender: &impl ModeRequestSender,
is_commandable: &impl IsChildCommandable,
opt_mode_reponse: Option<ModeResponse>,
mode_request_handler: F,
) -> Result<SubsystemHelperResult, ModeTreeHelperError> {
if let Some(reply) = opt_reply {
if self.handle_mode_reply(&reply)? {
if let Some(reply) = opt_mode_reponse {
if self.handle_children_mode_changed(reply)? {
if self.seq_exec_helper.state() == SequenceExecutionHelperState::Idle {
self.transition_to_target_keeping();
return Ok(SubsystemHelperResult::ModeCommanding(
@@ -484,9 +497,22 @@ impl SubsystemCommandingHelper {
Ok(SubsystemHelperResult::TargetKeeping)
}
ModeTreeHelperState::ModeCommanding => {
if self.seq_exec_helper.target_mode().is_none() {
return Err(ModeTreeHelperError::NoSequenceTableFound(None));
}
let seq_table = self
.sequence_tables
.0
.get(&self.seq_exec_helper.target_mode().unwrap());
if seq_table.is_none() {
return Err(ModeTreeHelperError::NoSequenceTableFound(
self.seq_exec_helper.target_mode(),
));
}
let result = self.seq_exec_helper.run(
&self.sequence_tables,
seq_table.unwrap(),
&mut self.children_mode_store,
mode_request_handler,
)?;
match result {
ModeCommandingResult::Done => {
@@ -520,18 +546,21 @@ impl SubsystemCommandingHelper {
if !entry.monitor_state {
continue;
}
let mut target_mode_violated = false;
let target_mode_violated = false;
self.children_mode_store.0.iter().for_each(|val| {
if val.id() == entry.common.target_id {
target_mode_violated =
if let Some(allowed_submode_mask) = entry.allowed_submode_mask() {
let fixed_bits = !allowed_submode_mask;
(val.mode_and_submode().mode() != entry.common.mode_submode.mode())
&& (val.mode_and_submode().submode() & fixed_bits
!= entry.common.mode_submode.submode() & fixed_bits)
} else {
val.mode_and_submode() != entry.common.mode_submode
};
// TODO: Solve this differently
//target_mode_violated =
/*
if let Some(allowed_submode_mask) = entry.allowed_submode_mask() {
let fixed_bits = !allowed_submode_mask;
(val.mode_and_submode().mode() != entry.common.mode_submode.mode())
&& (val.mode_and_submode().submode() & fixed_bits
!= entry.common.mode_submode.submode() & fixed_bits)
} else {
val.mode_and_submode() != entry.common.mode_submode
};
*/
}
});
if target_mode_violated {
@@ -553,48 +582,54 @@ impl SubsystemCommandingHelper {
// Handles a mode reply message and returns whether the reply completes a step of sequence
// commanding.
fn handle_mode_reply(
fn handle_children_mode_changed(
&mut self,
reply: &GenericMessage<ModeReply>,
mode_response: ModeResponse,
) -> Result<bool, ModeTreeHelperError> {
if !self.children_mode_store.has_component(reply.sender_id()) {
if !self
.children_mode_store
.has_component(mode_response.sender_id)
{
return Ok(false);
}
/*
let mut generic_mode_reply_handler =
|sender_id, mode_and_submode: Option<ModeAndSubmode>, success: bool| {
let mut partial_step_done = false;
// Tying the reply awaition to the request ID ensures that something like replies
// belonging to older requests do not interfere with the completion handling of
// the mode commanding. This is important for forced mode commands.
let mut handle_awaition = false;
if self.state == ModeTreeHelperState::ModeCommanding
&& self.active_internal_request_id.is_some()
&& reply.request_id() == self.active_internal_request_id.unwrap()
{
handle_awaition = true;
}
let still_awating_replies = self.children_mode_store.mode_reply_handler(
sender_id,
mode_and_submode,
handle_awaition,
);
if self.state == ModeTreeHelperState::ModeCommanding
&& handle_awaition
&& !still_awating_replies.unwrap_or(false)
{
self.seq_exec_helper.confirm_sequence_done();
self.update_internal_req_id();
partial_step_done = true;
}
if !success && self.state == ModeTreeHelperState::ModeCommanding {
// The user has to decide how to proceed.
self.state = ModeTreeHelperState::Idle;
return Err(ModeTreeHelperError::ModeCommmandFailure {
seq_table_index: self.seq_exec_helper.current_sequence_index(),
});
}
Ok(partial_step_done)
};
|sender_id, mode: Option<ModeRaw>, success: bool| {
*/
let mut partial_step_done = false;
// Tying the reply awaition to the request ID ensures that something like replies
// belonging to older requests do not interfere with the completion handling of
// the mode commanding. This is important for forced mode commands.
let mut handle_awaition = false;
if self.state == ModeTreeHelperState::ModeCommanding
&& self.active_internal_request_id.is_some()
&& mode_response.request_id == self.active_internal_request_id.unwrap()
{
handle_awaition = true;
}
let still_awating_replies = self.children_mode_store.mode_reply_handler(
mode_response.sender_id,
Some(mode_response.reported_mode),
handle_awaition,
);
if self.state == ModeTreeHelperState::ModeCommanding
&& handle_awaition
&& !still_awating_replies.unwrap_or(false)
{
self.seq_exec_helper.confirm_sequence_done();
self.update_internal_req_id();
partial_step_done = true;
}
if !mode_response.success && self.state == ModeTreeHelperState::ModeCommanding {
// The user has to decide how to proceed.
self.state = ModeTreeHelperState::Idle;
return Err(ModeTreeHelperError::ModeCommmandFailure {
seq_table_index: self.seq_exec_helper.current_sequence_index(),
});
}
Ok(partial_step_done)
//};
/*
match reply.message {
ModeReply::ModeInfo(mode_and_submode) => {
generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true)
@@ -609,22 +644,22 @@ impl SubsystemCommandingHelper {
generic_mode_reply_handler(reply.sender_id(), Some(reached), true)
}
}
*/
}
pub fn update_child_mode(
&mut self,
child: ComponentId,
mode: ModeAndSubmode,
mode: ModeRaw,
) -> Result<(), TargetNotInModeStoreError> {
let val_mut = self
.children_mode_store
.get_mut(child)
.ok_or(TargetNotInModeStoreError(child))?;
val_mut.mode_and_submode = mode;
val_mut.mode = mode;
Ok(())
}
}
*/
#[cfg(test)]
mod tests {
@@ -633,9 +668,13 @@ mod tests {
use super::*;
use crate::{
ComponentId, mode_tree::{
ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry, SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables,
}, queue::GenericTargetedMessagingError, request::RequestId, subsystem::{ModeCommandingResult, SequenceExecutionHelperState},
ComponentId,
mode_tree::{
ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry,
SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables,
},
request::RequestId,
subsystem::{ModeCommandingResult, SequenceExecutionHelperState},
};
#[derive(Debug)]
@@ -676,9 +715,12 @@ mod tests {
pub fn run(
&mut self,
command_list: &mut Vec<(ComponentId, ModeRaw)>,
) -> Result<ModeCommandingResult, GenericTargetedMessagingError> {
) -> Result<ModeCommandingResult, InvalidSequenceIndexError> {
self.execution_helper.run(
&self.seq_tables,
self.seq_tables
.0
.get(&self.execution_helper.target_mode().unwrap())
.unwrap(),
&mut self.mode_store,
|component_id, mode_raw| {
command_list.push((component_id, mode_raw));
@@ -690,9 +732,14 @@ mod tests {
// Assure that no unexpected behaviour occurs.
assert_eq!(
self.execution_helper
.run(&self.seq_tables, &mut self.mode_store, |_, _| {
panic!("should not have been called")
})
.run(
self.seq_tables
.0
.get(&self.execution_helper.target_mode().unwrap())
.unwrap(),
&mut self.mode_store,
|_, _| { panic!("should not have been called") }
)
.unwrap(),
ModeCommandingResult::Done
);