satrs-example/src/main.rs

@@ -9,7 +9,7 @@ use log::{info, warn};
 
 use crate::hk::AcsHkIds;
 use crate::logging::setup_logger;
-use crate::pus::test::PusService17TestHandler;
+use crate::pus::test::{PacketHandlerResult, PusService17TestHandler};
 use crate::pus::PusTcMpscRouter;
 use crate::requests::{Request, RequestWithToken};
 use crate::tmtc::{
@@ -31,6 +31,7 @@ use satrs_core::pus::verification::{
 };
 use satrs_core::pus::MpscTmtcInStoreSender;
 use satrs_core::seq_count::{SeqCountProviderSimple, SeqCountProviderSyncClonable};
+use satrs_core::spacepackets::tc::{GenericPusTcSecondaryHeader, PusTc};
 use satrs_core::spacepackets::{
     time::cds::TimeProvider,
     time::TimeWriter,
@@ -39,7 +40,7 @@ use satrs_core::spacepackets::{
 };
 use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
 use satrs_core::tmtc::AddressableId;
-use satrs_example::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT};
+use satrs_example::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT, TEST_EVENT};
 use std::collections::HashMap;
 use std::net::{IpAddr, SocketAddr};
 use std::sync::mpsc::{channel, TryRecvError};
@@ -338,11 +339,37 @@ fn main() {
         .name("PUS".to_string())
         .spawn(move || {
             loop {
+                let mut handled_pings = 0;
                 // TODO: Better error handling
-                let res = pus17_handler.periodic_operation();
-                res.expect("some PUS17 error");
+                let res = pus17_handler.handle_next_packet().unwrap();
+                match res {
+                    PacketHandlerResult::PingRequestHandled => {
+                        handled_pings += 1;
+                    }
+                    PacketHandlerResult::CustomSubservice => {
+                        let (buf, _) = pus17_handler.pus_tc_buf();
+                        let (tc, size) = PusTc::from_bytes(&buf).unwrap();
+                        if tc.subservice() == 128 {
+                            info!("Generating test event");
+                            event_sender
+                                .send((TEST_EVENT.into(), None))
+                                .expect("Sending test event failed");
+                            let start_token = pus17_handler
+                                .verification_handler()
+                                .start_success(token, Some(&stamp_buf))
+                                .expect("Error sending start success");
+                            pus17_handler
+                                .verification_handler()
+                                .completion_success(start_token, Some(&stamp_buf))
+                                .expect("Error sending completion success");
+                        }
+                    }
+                    PacketHandlerResult::Empty => {
                         thread::sleep(Duration::from_millis(400));
                     }
+                }
+                res.expect("some PUS17 error");
+            }
         })
         .unwrap();
     jh0.join().expect("Joining UDP TMTC server thread failed");

satrs-example/src/pus/mod.rs

@@ -45,7 +45,7 @@ pub struct PusServiceBase {
     verification_handler: StdVerifReporterWithSender,
     stamp_buf: [u8; 7],
     pus_buf: [u8; 2048],
-    handled_tcs: u32,
+    pus_size: usize,
 }
 
 impl PusServiceBase {
@@ -66,7 +66,7 @@ impl PusServiceBase {
             verification_handler,
             stamp_buf: [0; 7],
             pus_buf: [0; 2048],
-            handled_tcs: 0,
+            pus_size: 0,
         }
     }
 }

satrs-example/src/pus/test.rs

@@ -25,6 +25,12 @@ pub struct PusService17TestHandler {
     psb: PusServiceBase,
 }
 
+pub enum PacketHandlerResult {
+    PingRequestHandled,
+    CustomSubservice(VerificationToken<TcStateAccepted>),
+    Empty,
+}
+
 impl PusService17TestHandler {
     pub fn new(
         receiver: Receiver<AcceptedTc>,
@@ -46,6 +52,14 @@ impl PusService17TestHandler {
         }
     }
 
+    pub fn verification_handler(&mut self) -> &mut StdVerifReporterWithSender {
+        &mut self.psb.verification_handler
+    }
+
+    pub fn pus_tc_buf(&self) -> (&[u8], usize) {
+        (&self.psb.pus_buf, self.psb.pus_size)
+    }
+
     // TODO: Return errors which occured
     pub fn periodic_operation(&mut self) -> Result<u32, ()> {
         self.psb.handled_tcs = 0;
@@ -66,7 +80,33 @@ impl PusService17TestHandler {
             }
         }
     }
-    pub fn handle_one_tc(&mut self, addr: StoreAddr, token: VerificationToken<TcStateAccepted>) {
+
+    pub fn handle_next_packet(&mut self) -> Result<PacketHandlerResult, ()> {
+        match self.psb.tc_rx.try_recv() {
+            Ok((addr, token)) => {
+                if self.handle_one_tc(addr, token) {
+                    return Ok(PacketHandlerResult::PingRequestHandled);
+                } else {
+                    return Ok(PacketHandlerResult::CustomSubservice);
+                }
+            }
+            Err(e) => {
+                match e {
+                    TryRecvError::Empty => return Ok(PacketHandlerResult::Empty),
+                    TryRecvError::Disconnected => {
+                        // TODO: Replace panic by something cleaner
+                        panic!("PusService17Handler: Sender disconnected");
+                    }
+                }
+            }
+        }
+    }
+
+    pub fn handle_one_tc(
+        &mut self,
+        addr: StoreAddr,
+        token: VerificationToken<TcStateAccepted>,
+    ) -> bool {
         let time_provider = TimeProvider::from_now_with_u16_days().unwrap();
         // TODO: Better error handling
         {
@@ -104,7 +144,9 @@ impl PusService17TestHandler {
                 .completion_success(start_token, Some(&self.psb.stamp_buf))
                 .expect("Error sending completion success");
             self.psb.handled_tcs += 1;
+            true
         }
+        false
         // TODO: How to handle invalid subservice?
         // TODO: How do we handle custom code like this? Custom subservice handler via trait?
         // if tc.subservice() == 128 {