add scheduler service

src/pus/mod.rs

@@ -1,4 +1,5 @@
 pub mod action;
+pub mod scheduler;
 pub mod stack;
 pub mod test;
 
@@ -52,7 +53,7 @@ pub fn create_verification_reporter(owner_id: ComponentId, apid: Apid) -> Verifi
 pub struct PusTcMpscRouter {
     pub test_tc_sender: Sender<EcssTcAndToken>,
     // pub event_tc_sender: Sender<EcssTcAndToken>,
-    // pub sched_tc_sender: Sender<EcssTcAndToken>,
+    pub sched_tc_sender: Sender<EcssTcAndToken>,
     // pub hk_tc_sender: Sender<EcssTcAndToken>,
     pub action_tc_sender: Sender<EcssTcAndToken>,
     // pub mode_tc_sender: Sender<EcssTcAndToken>,

src/pus/scheduler.rs

@@ -0,0 +1,153 @@
+use std::sync::mpsc;
+use std::time::Duration;
+
+use crate::pus::create_verification_reporter;
+use log::{error, info, warn};
+use ops_sat_rs::config::components::PUS_SCHEDULER_SERVICE;
+use satrs::pool::{PoolProvider, StaticMemoryPool};
+use satrs::pus::scheduler::{PusScheduler, TcInfo};
+use satrs::pus::scheduler_srv::PusSchedServiceHandler;
+use satrs::pus::verification::VerificationReporter;
+use satrs::pus::{
+    EcssTcAndToken, EcssTcInVecConverter, MpscTcReceiver, PusPacketHandlerResult, PusServiceHelper,
+};
+use satrs::tmtc::{PacketAsVec, PacketInPool, PacketSenderWithSharedPool};
+use satrs::ComponentId;
+
+use super::HandlingStatus;
+
+pub trait TcReleaser {
+    fn release(&mut self, sender_id: ComponentId, enabled: bool, info: &TcInfo, tc: &[u8]) -> bool;
+}
+
+impl TcReleaser for PacketSenderWithSharedPool {
+    fn release(
+        &mut self,
+        sender_id: ComponentId,
+        enabled: bool,
+        _info: &TcInfo,
+        tc: &[u8],
+    ) -> bool {
+        if enabled {
+            let shared_pool = self.shared_pool.get_mut();
+            // Transfer TC from scheduler TC pool to shared TC pool.
+            let released_tc_addr = shared_pool
+                .0
+                .write()
+                .expect("locking pool failed")
+                .add(tc)
+                .expect("adding TC to shared pool failed");
+            self.sender
+                .send(PacketInPool::new(sender_id, released_tc_addr))
+                .expect("sending TC to TC source failed");
+        }
+        true
+    }
+}
+
+impl TcReleaser for mpsc::Sender<PacketAsVec> {
+    fn release(
+        &mut self,
+        sender_id: ComponentId,
+        enabled: bool,
+        _info: &TcInfo,
+        tc: &[u8],
+    ) -> bool {
+        if enabled {
+            // Send released TC to centralized TC source.
+            self.send(PacketAsVec::new(sender_id, tc.to_vec()))
+                .expect("sending TC to TC source failed");
+        }
+        true
+    }
+}
+
+pub struct SchedulingService {
+    pub pus_11_handler: PusSchedServiceHandler<
+        MpscTcReceiver,
+        mpsc::Sender<PacketAsVec>,
+        EcssTcInVecConverter,
+        VerificationReporter,
+        PusScheduler,
+    >,
+    pub sched_tc_pool: StaticMemoryPool,
+    pub releaser_buf: [u8; 4096],
+    pub tc_releaser: Box<dyn TcReleaser + Send>,
+}
+
+impl SchedulingService {
+    pub fn release_tcs(&mut self) {
+        let id = self.pus_11_handler.service_helper.id();
+        let releaser = |enabled: bool, info: &TcInfo, tc: &[u8]| -> bool {
+            self.tc_releaser.release(id, enabled, info, tc)
+        };
+
+        self.pus_11_handler
+            .scheduler_mut()
+            .update_time_from_now()
+            .unwrap();
+        let released_tcs = self
+            .pus_11_handler
+            .scheduler_mut()
+            .release_telecommands_with_buffer(
+                releaser,
+                &mut self.sched_tc_pool,
+                &mut self.releaser_buf,
+            )
+            .expect("releasing TCs failed");
+        if released_tcs > 0 {
+            info!("{released_tcs} TC(s) released from scheduler");
+        }
+    }
+
+    pub fn poll_and_handle_next_tc(&mut self, time_stamp: &[u8]) -> HandlingStatus {
+        match self
+            .pus_11_handler
+            .poll_and_handle_next_tc(time_stamp, &mut self.sched_tc_pool)
+        {
+            Ok(result) => match result {
+                PusPacketHandlerResult::RequestHandled => {}
+                PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
+                    warn!("PUS11 partial packet handling success: {e:?}")
+                }
+                PusPacketHandlerResult::CustomSubservice(invalid, _) => {
+                    warn!("PUS11 invalid subservice {invalid}");
+                }
+                PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
+                    warn!("PUS11: Subservice {subservice} not implemented");
+                }
+                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+            },
+            Err(error) => {
+                error!("PUS packet handling error: {error:?}")
+            }
+        }
+        HandlingStatus::HandledOne
+    }
+}
+
+pub fn create_scheduler_service(
+    tm_funnel_tx: mpsc::Sender<PacketAsVec>,
+    tc_source_sender: mpsc::Sender<PacketAsVec>,
+    pus_sched_rx: mpsc::Receiver<EcssTcAndToken>,
+    sched_tc_pool: StaticMemoryPool,
+) -> SchedulingService {
+    let scheduler = PusScheduler::new_with_current_init_time(Duration::from_secs(5))
+        .expect("Creating PUS Scheduler failed");
+    let pus_11_handler = PusSchedServiceHandler::new(
+        PusServiceHelper::new(
+            PUS_SCHEDULER_SERVICE.id(),
+            pus_sched_rx,
+            tm_funnel_tx,
+            create_verification_reporter(PUS_SCHEDULER_SERVICE.id(), PUS_SCHEDULER_SERVICE.apid),
+            EcssTcInVecConverter::default(),
+        ),
+        scheduler,
+    );
+    SchedulingService {
+        pus_11_handler,
+        sched_tc_pool,
+        releaser_buf: [0; 4096],
+        tc_releaser: Box::new(tc_source_sender),
+    }
+}

src/pus/stack.rs

@@ -3,7 +3,7 @@ use crate::pus::HandlingStatus;
 use derive_new::new;
 use satrs::spacepackets::time::{cds, TimeWriter};
 
-use super::{action::ActionServiceWrapper, TargetedPusService};
+use super::{action::ActionServiceWrapper, scheduler::SchedulingService, TargetedPusService};
 
 // use super::{
 //     action::ActionServiceWrapper, event::EventServiceWrapper, hk::HkServiceWrapper,
@@ -17,7 +17,7 @@ pub struct PusStack {
     // hk_srv_wrapper: HkServiceWrapper<TmSender, TcInMemConverter>,
     // event_srv: EventServiceWrapper<TmSender, TcInMemConverter>,
     action_srv_wrapper: ActionServiceWrapper,
-    // schedule_srv: SchedulingServiceWrapper<TmSender, TcInMemConverter>,
+    schedule_srv: SchedulingService,
     // mode_srv: ModeServiceWrapper<TmSender, TcInMemConverter>,
 }
 
@@ -25,7 +25,7 @@ impl PusStack {
     pub fn periodic_operation(&mut self) {
         // Release all telecommands which reached their release time before calling the service
         // handlers.
-        // self.schedule_srv.release_tcs();
+        self.schedule_srv.release_tcs();
         let time_stamp = cds::CdsTime::now_with_u16_days()
             .expect("time stamp generation error")
             .to_vec()
@@ -48,7 +48,11 @@ impl PusStack {
                 self.test_srv.poll_and_handle_next_packet(&time_stamp),
                 None,
             );
-            // is_srv_finished(self.schedule_srv.poll_and_handle_next_tc(&time_stamp), None);
+            is_srv_finished(
+                11,
+                self.schedule_srv.poll_and_handle_next_tc(&time_stamp),
+                None,
+            );
             // is_srv_finished(self.event_srv.poll_and_handle_next_tc(&time_stamp), None);
             is_srv_finished(
                 8,