Files
sat-rs/satrs/src/mode_tree.rs
T

718 lines
23 KiB
Rust

use alloc::vec::Vec;
use hashbrown::HashMap;
use crate::{ComponentId, mode::ModeRaw};
#[cfg(feature = "alloc")]
pub use alloc_mod::*;
/// Common trait for node modes which can have mode parents or mode children.
pub trait ModeNode {
fn id(&self) -> ComponentId;
}
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum TableEntryType {
/// Target table containing information of the expected children modes for given mode.
Target,
/// Sequence table which contains information about how to reach a target table, including
/// the order of the sequences.
Sequence,
}
/// Common fields required for both target and sequence table entries.
///
/// The most important parameters here are the target ID which this entry belongs to, and the mode
/// and submode the entry either will be commanded to for sequence table entries or which will be
/// monitored for target table entries.
#[derive(Debug, Copy, Clone)]
pub struct ModeTableEntryCommon {
/// Name of respective table entry.
pub name: &'static str,
/// Target component ID.
pub target_id: ComponentId,
/// Has a different meaning depending on whether this is a sequence table or a target table.
///
/// - For sequence tables, this denotes the mode which will be commanded
/// - For target tables, this is the mode which the target children should have and which
/// might be monitored depending on configuration.
pub mode: ModeRaw,
}
/// An entry for the target tables.
#[derive(Debug)]
pub struct TargetTableEntry {
pub common: ModeTableEntryCommon,
/// To allow multiple modes, you can specify mask bits which are ignored during mode
/// checking.
pub ignored_bits: Option<u32>,
pub monitor_state: bool,
}
impl TargetTableEntry {
pub fn new(name: &'static str, target_id: ComponentId, mode: ModeRaw) -> Self {
Self {
common: ModeTableEntryCommon {
name,
target_id,
mode,
},
ignored_bits: None,
monitor_state: true,
}
}
pub fn new_with_ignored_bits(
name: &'static str,
target_id: ComponentId,
mode: ModeRaw,
ignored_bits: Option<u32>,
) -> Self {
Self {
common: ModeTableEntryCommon {
name,
target_id,
mode,
},
ignored_bits,
monitor_state: true,
}
}
}
/// An entry for the sequence tables.
///
/// The [Self::check_success] field specifies that a mode sequence executor should check that the
/// target mode was actually reached before executing the next sequence.
#[derive(Debug)]
pub struct SequenceTableEntry {
pub common: ModeTableEntryCommon,
pub check_success: bool,
}
impl SequenceTableEntry {
pub fn new(
name: &'static str,
target_id: ComponentId,
mode: ModeRaw,
check_success: bool,
) -> Self {
Self {
common: ModeTableEntryCommon {
name,
target_id,
mode,
},
check_success,
}
}
delegate::delegate! {
to self.common {
}
}
}
#[derive(Debug, thiserror::Error)]
#[error("target {0} not in mode store")]
pub struct TargetNotInModeStoreError(pub ComponentId);
/// Mode store value type.
#[derive(Debug, Copy, Clone)]
pub struct ModeStoreValue {
/// ID of the mode component.
id: ComponentId,
/// Current mode and submode of the component.
pub mode: ModeRaw,
/// State information to track whether a reply should be awaited for the mode component.
pub awaiting_reply: bool,
}
impl ModeStoreValue {
pub fn new(id: ComponentId, mode: ModeRaw) -> Self {
Self {
id,
mode,
awaiting_reply: false,
}
}
pub fn id(&self) -> ComponentId {
self.id
}
pub fn mode(&self) -> ModeRaw {
self.mode
}
}
pub trait SequenceTableProvider {
fn sequence_at_index(&self, index: u8) -> Option<&[SequenceTableEntry]>;
fn number_of_sequences(&self) -> usize;
}
#[derive(Debug, thiserror::Error)]
#[error("mode store is full")]
pub struct FullError;
pub trait ModeStoreProvider {
fn add_component(
&mut self,
target_id: ComponentId,
initial_mode: ModeRaw,
) -> Result<(), FullError>;
fn has_component(&self, target_id: ComponentId) -> bool;
fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue>;
fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue>;
/// Generic handler for mode replies received from child components.
///
/// Implementation should clear the awaition flag if the `handle_reply_awaition` argument is
/// true and returns whether any children are still awaiting replies.
fn mode_reply_handler_with_reply_awaition(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
) -> bool {
self.mode_reply_handler(sender_id, reported_mode_and_submode, true)
.unwrap_or(false)
}
fn mode_reply_handler_without_reply_awaition(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
) {
self.mode_reply_handler(sender_id, reported_mode_and_submode, false);
}
/// This function should return whether any children are still awaiting replies.
fn mode_reply_handler(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
with_reply_awaition: bool,
) -> Option<bool>;
fn set_reply_awaition_flag(&mut self, target_id: ComponentId);
}
impl<const N: usize> ModeStoreProvider for heapless::Vec<ModeStoreValue, N> {
fn add_component(
&mut self,
target_id: ComponentId,
initial_mode: ModeRaw,
) -> Result<(), FullError> {
if self.is_full() {
return Err(FullError);
}
self.push(ModeStoreValue::new(target_id, initial_mode))
.unwrap();
Ok(())
}
fn has_component(&self, target_id: ComponentId) -> bool {
self.iter().any(|val| val.id == target_id)
}
fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
self.iter().find(|val| val.id == target_id)
}
fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
self.iter_mut().find(|val| val.id == target_id)
}
fn mode_reply_handler(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
with_reply_awaition: bool,
) -> Option<bool> {
let mut still_awating_replies = None;
if with_reply_awaition {
still_awating_replies = Some(false);
}
self.iter_mut().for_each(|val| {
if val.id() == sender_id {
if let Some(mode) = reported_mode_and_submode {
val.mode = mode;
}
if with_reply_awaition {
val.awaiting_reply = false;
}
}
if with_reply_awaition && val.awaiting_reply {
still_awating_replies = Some(true);
}
});
still_awating_replies
}
fn set_reply_awaition_flag(&mut self, target_id: ComponentId) {
self.iter_mut().for_each(|val| {
if val.id() == target_id {
val.awaiting_reply = true;
}
});
}
}
pub struct HeaplessModeStoreProvider<const N: usize>(pub heapless::Vec<ModeStoreValue, N>);
impl<const N: usize> ModeStoreProvider for HeaplessModeStoreProvider<N> {
fn add_component(
&mut self,
target_id: ComponentId,
initial_mode: ModeRaw,
) -> Result<(), FullError> {
self.0.add_component(target_id, initial_mode)
}
fn has_component(&self, target_id: ComponentId) -> bool {
self.0.has_component(target_id)
}
fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
self.0.get(target_id)
}
fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
self.0.get_mut(target_id)
}
fn mode_reply_handler(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
with_reply_awaition: bool,
) -> Option<bool> {
self.0
.mode_reply_handler(sender_id, reported_mode_and_submode, with_reply_awaition)
}
fn set_reply_awaition_flag(&mut self, target_id: ComponentId) {
self.0.set_reply_awaition_flag(target_id)
}
}
#[cfg(feature = "alloc")]
pub mod alloc_mod {
use crate::subsystem::ModeRaw;
use super::*;
#[derive(Debug)]
pub struct TargetTablesMapValue {
/// Name for a given mode table entry.
pub name: &'static str,
/// Optional fallback mode if the target mode can not be kept.
pub fallback_mode: Option<ModeRaw>,
/// These are the rows of the a target table.
pub entries: Vec<TargetTableEntry>,
}
impl TargetTablesMapValue {
pub fn new(name: &'static str, fallback_mode: Option<ModeRaw>) -> Self {
Self {
name,
fallback_mode,
entries: Default::default(),
}
}
pub fn add_entry(&mut self, entry: TargetTableEntry) {
self.entries.push(entry);
}
}
/// One sequence of a [SequenceTablesMapValue] in a [SequenceModeTables].
///
/// It contains all mode requests which need to be executed for a sequence step and it also
/// associates a [Self::name] with the sequence.
#[derive(Debug)]
pub struct SequenceTableMapTable {
/// Name for a given mode sequence.
pub name: &'static str,
/// These are the rows of the a sequence table.
pub entries: Vec<SequenceTableEntry>,
}
impl SequenceTableMapTable {
pub fn new(name: &'static str) -> Self {
Self {
name,
entries: Default::default(),
}
}
pub fn add_entry(&mut self, entry: SequenceTableEntry) {
self.entries.push(entry);
}
}
/// A sequence table entry.
///
/// This is simply a list of [SequenceTableMapTable]s which also associates a [Self::name]
/// with the sequence. The order of sub-tables in the list also specifies the execution order
/// in the mode sequence.
#[derive(Debug)]
pub struct SequenceTablesMapValue {
/// Name for a given mode sequence.
pub name: &'static str,
/// Each sequence can consists of multiple sequences that are executed consecutively.
pub entries: Vec<SequenceTableMapTable>,
}
impl SequenceTablesMapValue {
pub fn new(name: &'static str) -> Self {
Self {
name,
entries: Default::default(),
}
}
pub fn add_sequence_table(&mut self, entry: SequenceTableMapTable) {
self.entries.push(entry);
}
}
impl SequenceTableProvider for SequenceTablesMapValue {
fn sequence_at_index(&self, index: u8) -> Option<&[SequenceTableEntry]> {
self.entries
.get(index as usize)
.map(|table| table.entries.as_slice())
}
fn number_of_sequences(&self) -> usize {
self.entries.len()
}
}
#[derive(Debug, Default)]
pub struct TargetModeTables(pub HashMap<ModeRaw, TargetTablesMapValue>);
impl TargetModeTables {
pub fn name(&self, mode: ModeRaw) -> Option<&'static str> {
self.0.get(&mode).map(|value| value.name)
}
}
impl SequenceModeTables {
pub fn name(&self, mode: ModeRaw) -> Option<&'static str> {
self.0.get(&mode).map(|value| value.name)
}
pub fn name_of_sequence(&self, mode: ModeRaw, seq_idx: usize) -> Option<&'static str> {
self.0
.get(&mode)
.map(|value| value.entries.get(seq_idx).map(|v| v.name))?
}
}
/// This is the core data structure used to store mode sequence tables.
///
/// A mode sequence table specifies which commands have to be sent in which order
/// to reach a certain [mode][ModeRaw]. Therefore, it simply maps a [mode][ModeRaw] to a [SequenceTablesMapValue].
#[derive(Debug, Default)]
pub struct SequenceModeTables(pub HashMap<ModeRaw, SequenceTablesMapValue>);
/// Mode store which tracks the [mode information][ModeStoreValue] inside a [Vec]
#[derive(Debug, Default)]
pub struct ModeStoreVec(pub alloc::vec::Vec<ModeStoreValue>);
impl<'a> IntoIterator for &'a ModeStoreVec {
type Item = &'a ModeStoreValue;
type IntoIter = std::slice::Iter<'a, ModeStoreValue>;
fn into_iter(self) -> Self::IntoIter {
self.0.iter()
}
}
impl<'a> IntoIterator for &'a mut ModeStoreVec {
type Item = &'a mut ModeStoreValue;
type IntoIter = std::slice::IterMut<'a, ModeStoreValue>;
fn into_iter(self) -> Self::IntoIter {
self.0.iter_mut()
}
}
/// Mode store which tracks the mode information inside a [hashbrown::HashMap]
#[derive(Debug, Default)]
pub struct ModeStoreMap(pub hashbrown::HashMap<ComponentId, ModeStoreValue>);
impl<'a> IntoIterator for &'a ModeStoreMap {
type Item = (&'a ComponentId, &'a ModeStoreValue);
type IntoIter = hashbrown::hash_map::Iter<'a, ComponentId, ModeStoreValue>;
fn into_iter(self) -> Self::IntoIter {
self.0.iter()
}
}
impl ModeStoreProvider for alloc::vec::Vec<ModeStoreValue> {
fn add_component(
&mut self,
target_id: ComponentId,
mode: ModeRaw,
) -> Result<(), FullError> {
self.push(ModeStoreValue::new(target_id, mode));
Ok(())
}
fn has_component(&self, target_id: ComponentId) -> bool {
self.iter().any(|val| val.id == target_id)
}
fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
self.iter().find(|val| val.id == target_id)
}
fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
self.iter_mut().find(|val| val.id == target_id)
}
fn mode_reply_handler(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
handle_reply_awaition: bool,
) -> Option<bool> {
let mut still_awating_replies = None;
if handle_reply_awaition {
still_awating_replies = Some(false);
}
self.iter_mut().for_each(|val| {
if val.id() == sender_id {
if let Some(mode) = reported_mode_and_submode {
val.mode = mode;
}
if handle_reply_awaition {
val.awaiting_reply = false;
}
}
if handle_reply_awaition && val.awaiting_reply {
still_awating_replies = Some(true);
}
});
still_awating_replies
}
fn set_reply_awaition_flag(&mut self, target_id: ComponentId) {
self.iter_mut().for_each(|val| {
if val.id() == target_id {
val.awaiting_reply = true;
}
});
}
}
impl ModeStoreProvider for ModeStoreVec {
fn add_component(
&mut self,
target_id: ComponentId,
mode: ModeRaw,
) -> Result<(), FullError> {
self.0.add_component(target_id, mode)
}
fn has_component(&self, target_id: ComponentId) -> bool {
self.0.has_component(target_id)
}
fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
self.0.get(target_id)
}
fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
self.0.get_mut(target_id)
}
fn mode_reply_handler(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
handle_reply_awaition: bool,
) -> Option<bool> {
self.0
.mode_reply_handler(sender_id, reported_mode_and_submode, handle_reply_awaition)
}
fn set_reply_awaition_flag(&mut self, target_id: ComponentId) {
self.0.set_reply_awaition_flag(target_id)
}
}
impl ModeStoreProvider for ModeStoreMap {
fn add_component(
&mut self,
target_id: ComponentId,
mode: ModeRaw,
) -> Result<(), FullError> {
self.0
.insert(target_id, ModeStoreValue::new(target_id, mode));
Ok(())
}
fn has_component(&self, target_id: ComponentId) -> bool {
self.0.contains_key(&target_id)
}
fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
self.0.get(&target_id)
}
fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
self.0.get_mut(&target_id)
}
fn mode_reply_handler(
&mut self,
sender_id: ComponentId,
reported_mode_and_submode: Option<ModeRaw>,
handle_reply_awaition: bool,
) -> Option<bool> {
let mut still_awating_replies = None;
if handle_reply_awaition {
still_awating_replies = Some(false);
}
for val in self.0.values_mut() {
if val.id() == sender_id {
if let Some(mode) = reported_mode_and_submode {
val.mode = mode;
}
if handle_reply_awaition {
val.awaiting_reply = false;
}
}
if handle_reply_awaition && val.awaiting_reply {
still_awating_replies = Some(true);
}
}
still_awating_replies
}
fn set_reply_awaition_flag(&mut self, target_id: ComponentId) {
self.0.iter_mut().for_each(|(_, val)| {
if val.id() == target_id {
val.awaiting_reply = true;
}
});
}
}
}
#[cfg(test)]
mod tests {
use super::*;
fn generic_test(mode_store: &mut impl ModeStoreProvider) {
mode_store.add_component(1, 0).unwrap();
mode_store.add_component(2, 1).unwrap();
assert!(mode_store.has_component(1));
assert!(mode_store.has_component(2));
assert_eq!(mode_store.get(1).unwrap().mode(), 0);
assert!(!mode_store.get(1).unwrap().awaiting_reply);
assert!(!mode_store.get(2).unwrap().awaiting_reply);
assert_eq!(mode_store.get(1).unwrap().id, 1);
assert_eq!(mode_store.get(2).unwrap().id, 2);
assert!(mode_store.get(3).is_none());
assert!(mode_store.get_mut(3).is_none());
}
fn generic_reply_handling_with_reply_awaition(mode_store: &mut impl ModeStoreProvider) {
mode_store.add_component(1, 0).unwrap();
mode_store.add_component(2, 1).unwrap();
mode_store.get_mut(1).unwrap().awaiting_reply = true;
mode_store.get_mut(2).unwrap().awaiting_reply = true;
let mut reply_awation_pending =
mode_store.mode_reply_handler_with_reply_awaition(1, Some(2));
assert!(reply_awation_pending);
reply_awation_pending = mode_store.mode_reply_handler_with_reply_awaition(2, None);
assert!(!reply_awation_pending);
assert!(!mode_store.get(1).unwrap().awaiting_reply);
assert!(!mode_store.get(2).unwrap().awaiting_reply);
assert_eq!(mode_store.get(1).unwrap().mode(), 2);
assert_eq!(mode_store.get(2).unwrap().mode(), 1);
}
fn generic_reply_handling_test_no_reply_awaition(mode_store: &mut impl ModeStoreProvider) {
mode_store.add_component(1, 0).unwrap();
mode_store.add_component(2, 1).unwrap();
mode_store.get_mut(1).unwrap().awaiting_reply = true;
mode_store.get_mut(2).unwrap().awaiting_reply = true;
mode_store.mode_reply_handler_without_reply_awaition(1, Some(2));
mode_store.mode_reply_handler_without_reply_awaition(2, None);
assert!(mode_store.get(1).unwrap().awaiting_reply);
assert!(mode_store.get(2).unwrap().awaiting_reply);
assert_eq!(mode_store.get(1).unwrap().mode(), 2);
assert_eq!(mode_store.get(2).unwrap().mode(), 1);
}
fn generic_reply_handling_with_reply_awaition_2(mode_store: &mut impl ModeStoreProvider) {
mode_store.add_component(1, 0).unwrap();
mode_store.add_component(2, 1).unwrap();
mode_store.get_mut(1).unwrap().awaiting_reply = true;
mode_store.get_mut(2).unwrap().awaiting_reply = true;
let mut reply_awation_pending = mode_store.mode_reply_handler(1, Some(2), true);
assert!(reply_awation_pending.unwrap());
reply_awation_pending = mode_store.mode_reply_handler(2, None, true);
assert!(!reply_awation_pending.unwrap());
assert!(!mode_store.get(1).unwrap().awaiting_reply);
assert!(!mode_store.get(2).unwrap().awaiting_reply);
assert_eq!(mode_store.get(1).unwrap().mode(), 2);
assert_eq!(mode_store.get(2).unwrap().mode(), 1);
}
#[test]
fn test_vec_mode_store() {
let mut mode_store = ModeStoreVec::default();
generic_test(&mut mode_store);
}
#[test]
fn test_map_mode_store() {
let mut mode_store = ModeStoreMap::default();
generic_test(&mut mode_store);
}
#[test]
fn test_generic_reply_handler_vec_with_reply_awaition() {
let mut mode_store = ModeStoreVec::default();
generic_reply_handling_with_reply_awaition(&mut mode_store);
}
#[test]
fn test_generic_reply_handler_vec_with_reply_awaition_2() {
let mut mode_store = ModeStoreVec::default();
generic_reply_handling_with_reply_awaition_2(&mut mode_store);
}
#[test]
fn test_generic_reply_handler_map_with_reply_awaition() {
let mut mode_store = ModeStoreMap::default();
generic_reply_handling_with_reply_awaition(&mut mode_store);
}
#[test]
fn test_generic_reply_handler_map_with_reply_awaition_2() {
let mut mode_store = ModeStoreMap::default();
generic_reply_handling_with_reply_awaition_2(&mut mode_store);
}
#[test]
fn test_generic_reply_handler_vec_no_reply_awaition() {
let mut mode_store = ModeStoreVec::default();
generic_reply_handling_test_no_reply_awaition(&mut mode_store);
}
#[test]
fn test_generic_reply_handler_map_no_reply_awaition() {
let mut mode_store = ModeStoreMap::default();
generic_reply_handling_test_no_reply_awaition(&mut mode_store);
}
}