finished very basic implementation

satrs/Cargo.toml

@@ -14,7 +14,7 @@ categories = ["aerospace", "aerospace::space-protocols", "no-std", "hardware-sup
 
 [dependencies]
 satrs-shared = { version = "0.2", path = "../satrs-shared" }
-spacepackets = { version = "0.17", default-features = false }
+spacepackets = { version = "0.17", git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git", default-features = false }
 
 delegate = "0.13"
 paste = "1"

satrs/src/ccsds/scheduler.rs

@@ -1,45 +1,58 @@
+//! # CCSDS Telecommand Scheduler.
+#![deny(missing_docs)]
 use core::{hash::Hash, time::Duration};
 
 #[cfg(feature = "alloc")]
 pub use alloc_mod::*;
 use spacepackets::{
-    ByteConversionError, CcsdsPacketIdAndPsc,
+    CcsdsPacketIdAndPsc,
     time::{TimestampError, UnixTime},
 };
 
+/// Generic CCSDS scheduling errors.
 #[derive(Debug, Clone, PartialEq, Eq, thiserror::Error)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum ScheduleError {
     /// The release time is within the time-margin added on top of the current time.
     /// The first parameter is the current time, the second one the time margin, and the third one
     /// the release time.
     #[error("release time in margin")]
     ReleaseTimeInTimeMargin {
+        /// Current time.
         current_time: UnixTime,
+        /// Configured time margin.
         time_margin: Duration,
+        /// Release time.
         release_time: UnixTime,
     },
     /// Nested time-tagged commands are not allowed.
     #[error("nested scheduled tc")]
     NestedScheduledTc,
+    /// TC data is empty.
     #[error("tc data empty")]
     TcDataEmpty,
-    #[error("scheduler is full")]
-    Full,
+    /// Scheduler is full, packet number limit reached.
+    #[error("scheduler is full, packet number limit reached")]
+    PacketLimitReached,
+    /// Scheduler is full, numver of bytes limit reached.
+    #[error("scheduler is full, number of bytes limit reached")]
+    ByteLimitReached,
+    /// Timestamp error.
     #[error("timestamp error: {0}")]
     TimestampError(#[from] TimestampError),
-    #[error("wrong subservice number {0}")]
-    WrongSubservice(u8),
-    #[error("wrong service number {0}")]
-    WrongService(u8),
-    #[error("byte conversion error: {0}")]
-    ByteConversionError(#[from] ByteConversionError),
 }
 
+/// Packet ID used for identifying scheduled packets.
+///
+/// Right now, this ID can be determined from the packet without requiring external input
+/// or custom data fields in the CCSDS space pacekt.
 #[derive(Debug, PartialEq, Eq, Clone)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
 pub struct CcsdsSchedulePacketId {
+    /// Base ID.
     pub base: CcsdsPacketIdAndPsc,
+    /// Optional checksum of the packet.
     pub crc16: Option<u16>,
 }
 
@@ -50,32 +63,72 @@ impl Hash for CcsdsSchedulePacketId {
     }
 }
 
+/// Modules requiring [alloc] support.
 #[cfg(feature = "alloc")]
 pub mod alloc_mod {
     use core::time::Duration;
     #[cfg(feature = "std")]
     use std::time::SystemTimeError;
 
+    use alloc::collections::btree_map;
     use spacepackets::{CcsdsPacketIdAndPsc, CcsdsPacketReader, time::UnixTime};
 
     use crate::ccsds::scheduler::CcsdsSchedulePacketId;
 
+    /// The scheduler can be configured to have bounds for both the number of packets
+    /// and the total number of bytes used by scheduled packets.
+    ///
+    /// This can be used to avoid memory exhaustion in systems with limited resources or under
+    /// heavy workloads.
+    #[derive(Default, Debug)]
+    #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub struct Limits {
+        /// Maximum number of scheduled packets.
+        pub packets: Option<usize>,
+        /// Maximum total number of bytes used by scheduled packets.
+        pub bytes: Option<usize>,
+    }
+
+    impl Limits {
+        /// Check if no limits are set.
+        pub fn has_no_limits(&self) -> bool {
+            self.packets.is_none() || self.bytes.is_none()
+        }
+    }
+
+    /// Fill count of the scheduler.
+    #[derive(Default, Debug)]
+    #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub struct FillCount {
+        /// Number of scheduled packets.
+        pub packets: usize,
+        /// Total number of bytes used by scheduled packets.
+        pub bytes: usize,
+    }
+
+    /// Simple CCSDS scheduler implementation.
+    ///
+    /// Relies of [alloc] support but limits the number of scheduled packets.
+    #[derive(Debug)]
     pub struct CcsdsScheduler {
         tc_map: alloc::collections::BTreeMap<
             UnixTime,
             alloc::vec::Vec<(CcsdsSchedulePacketId, alloc::vec::Vec<u8>)>,
         >,
-        packet_limit: usize,
+        limits: Limits,
         pub(crate) current_time: UnixTime,
         time_margin: Duration,
         enabled: bool,
     }
 
     impl CcsdsScheduler {
-        pub fn new(current_time: UnixTime, packet_limit: usize, time_margin: Duration) -> Self {
+        /// Create a new CCSDS scheduler.
+        pub fn new(current_time: UnixTime, limits: Limits, time_margin: Duration) -> Self {
             Self {
                 tc_map: alloc::collections::BTreeMap::new(),
-                packet_limit,
+                limits,
                 current_time,
                 time_margin,
                 enabled: true,
@@ -85,12 +138,28 @@ pub mod alloc_mod {
         /// Like [Self::new], but sets the `init_current_time` parameter to the current system time.
         #[cfg(feature = "std")]
         pub fn new_with_current_init_time(
-            packet_limit: usize,
+            limits: Limits,
             time_margin: Duration,
         ) -> Result<Self, SystemTimeError> {
-            Ok(Self::new(UnixTime::now()?, packet_limit, time_margin))
+            Ok(Self::new(UnixTime::now()?, limits, time_margin))
         }
 
+        /// Current fill count: number of scheduled packets and total number of bytes.
+        ///
+        /// The first returned value is the number of scheduled packets, the second one is the
+        /// byte count.
+        pub fn current_fill_count(&self) -> FillCount {
+            let mut fill_count = FillCount::default();
+            for value in self.tc_map.values() {
+                for (_, raw_scheduled_tc) in value {
+                    fill_count.packets += 1;
+                    fill_count.bytes += raw_scheduled_tc.len();
+                }
+            }
+            fill_count
+        }
+
+        /// Current number of scheduled entries.
         pub fn num_of_entries(&self) -> usize {
             self.tc_map
                 .values()
@@ -98,50 +167,126 @@ pub mod alloc_mod {
                 .sum()
         }
 
+        /// Enable the scheduler.
         #[inline]
         pub fn enable(&mut self) {
             self.enabled = true;
         }
 
+        /// Disable the scheduler.
         #[inline]
         pub fn disable(&mut self) {
             self.enabled = false;
         }
 
+        /// Update the current time.
         #[inline]
         pub fn update_time(&mut self, current_time: UnixTime) {
             self.current_time = current_time;
         }
 
+        /// Current time.
         #[inline]
         pub fn current_time(&self) -> &UnixTime {
             &self.current_time
         }
 
+        fn common_check(
+            &mut self,
+            release_time: UnixTime,
+            packet_size: usize,
+        ) -> Result<(), super::ScheduleError> {
+            if !self.limits.has_no_limits() {
+                let fill_count = self.current_fill_count();
+                if let Some(max_bytes) = self.limits.bytes {
+                    if fill_count.bytes + packet_size >= max_bytes {
+                        return Err(super::ScheduleError::ByteLimitReached);
+                    }
+                }
+                if let Some(max_packets) = self.limits.packets {
+                    if fill_count.packets + 1 >= max_packets {
+                        return Err(super::ScheduleError::PacketLimitReached);
+                    }
+                }
+            }
+            if release_time < self.current_time + self.time_margin {
+                return Err(super::ScheduleError::ReleaseTimeInTimeMargin {
+                    current_time: self.current_time,
+                    time_margin: self.time_margin,
+                    release_time,
+                });
+            }
+            Ok(())
+        }
+
+        /// Insert a telecommand using an existing [CcsdsPacketReader].
         pub fn insert_telecommand_with_reader(
             &mut self,
             reader: &CcsdsPacketReader,
             release_time: UnixTime,
         ) -> Result<(), super::ScheduleError> {
-            if self.num_of_entries() + 1 >= self.packet_limit {
-                return Err(super::ScheduleError::Full);
-            }
+            self.common_check(release_time, reader.packet_len())?;
             let base_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(reader);
+            let checksum = reader.checksum();
+            let packet_id_scheduling = CcsdsSchedulePacketId {
+                base: base_id,
+                crc16: checksum,
+            };
+            self.insert_telecommand(packet_id_scheduling, reader.raw_data(), release_time)?;
 
             Ok(())
         }
 
-        // TODO: Implementation
+        /// Insert a raw telecommand, assuming the user has already extracted the
+        /// [CcsdsSchedulePacketId]
         pub fn insert_telecommand(
             &mut self,
-            packet_id: CcsdsSchedulePacketId,
+            packet_id_scheduling: CcsdsSchedulePacketId,
             raw_packet: &[u8],
             release_time: UnixTime,
         ) -> Result<(), super::ScheduleError> {
-            if self.num_of_entries() + 1 >= self.packet_limit {
-                return Err(super::ScheduleError::Full);
+            self.common_check(release_time, raw_packet.len())?;
+            match self.tc_map.entry(release_time) {
+                btree_map::Entry::Vacant(e) => {
+                    e.insert(alloc::vec![(packet_id_scheduling, raw_packet.to_vec())]);
+                }
+                btree_map::Entry::Occupied(mut v) => {
+                    v.get_mut()
+                        .push((packet_id_scheduling, raw_packet.to_vec()));
+                }
             }
             Ok(())
         }
+
+        /// Release all telecommands which should be released based on the current time.
+        pub fn release_telecommands<R: FnMut(bool, &CcsdsSchedulePacketId, &[u8])>(
+            &mut self,
+            mut releaser: R,
+        ) {
+            let tcs_to_release = self.telecommands_to_release();
+            for tc_group in tcs_to_release {
+                for (packet_id, raw_tc) in tc_group.1 {
+                    releaser(self.enabled, packet_id, raw_tc);
+                }
+            }
+            self.tc_map.retain(|k, _| k > &self.current_time);
+        }
+
+        /// Retrieve all telecommands which should be released based on the current time.
+        pub fn telecommands_to_release(
+            &self,
+        ) -> btree_map::Range<
+            '_,
+            UnixTime,
+            alloc::vec::Vec<(CcsdsSchedulePacketId, alloc::vec::Vec<u8>)>,
+        > {
+            self.tc_map.range(..=self.current_time)
+        }
     }
 }
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn test_basic() {}
+}