Merge pull request 'Move Images Commad and Bugfixes' (#28) from move-images-command into main

Reviewed-on: #28
Reviewed-by: lkoester <st167799@stud.uni-stuttgart.de>

pytmtc/opssat_tmtc/camera.py

@@ -1,5 +1,12 @@
 import enum
+from typing import List
+
+from spacepackets.ecss import PusTc
+from tmtccmd.config import CmdTreeNode
 from pydantic import BaseModel
+from tmtccmd.tmtc import DefaultPusQueueHelper
+
+from opssat_tmtc.common import EXPERIMENT_APID, UniqueId, make_action_cmd_header
 
 
 class ActionId(enum.IntEnum):
@@ -18,3 +25,72 @@ class CameraParameters(BaseModel):
     P: bool
     E: int
     W: int
+
+
+def create_camera_node() -> CmdTreeNode:
+    cam_node = CmdTreeNode("cam", "OPS-SAT IMS1000 batch handler commands")
+    cam_node.add_child(
+        CmdTreeNode("default_single", "Default Single Image Camera Parameters")
+    )
+    cam_node.add_child(
+        CmdTreeNode("balanced_single", "Balanced Single Image Camera Parameters")
+    )
+    cam_node.add_child(
+        CmdTreeNode(
+            "default_single_flatsat",
+            "Default Single Image Camera Parameters for use on FlatSat",
+        )
+    )
+    cam_node.add_child(
+        CmdTreeNode(
+            "balanced_single_flatsat",
+            "Balanced Single Image Camera Parameters for use on FlatSat",
+        )
+    )
+    cam_node.add_child(
+        CmdTreeNode("custom_params", "Custom Camera Parameters as specified from file")
+    )
+    return cam_node
+
+
+def create_cam_cmd(q: DefaultPusQueueHelper, cmd_path: List[str]):
+
+    assert len(cmd_path) >= 1
+    q.add_log_cmd(
+        "Sending PUS take image action request for command " + cmd_path[0] + " params."
+    )
+    data = bytearray()
+    if cmd_path[0] == "default_single":
+        data.extend(
+            make_action_cmd_header(UniqueId.CameraHandler, ActionId.DEFAULT_SINGLE)
+        )
+    elif cmd_path[0] == "balanced_single":
+        data.extend(
+            make_action_cmd_header(UniqueId.CameraHandler, ActionId.BALANCED_SINGLE)
+        )
+    elif cmd_path[0] == "default_single_flatsat":
+        data.extend(
+            make_action_cmd_header(
+                UniqueId.CameraHandler, ActionId.DEFAULT_SINGLE_FLATSAT
+            )
+        )
+    elif cmd_path[0] == "balanced_single_flatsat":
+        data.extend(
+            make_action_cmd_header(
+                UniqueId.CameraHandler, ActionId.BALANCED_SNGLE_FLATSAT
+            )
+        )
+    elif cmd_path[0] == "custom":
+        data.extend(
+            make_action_cmd_header(UniqueId.CameraHandler, ActionId.CUSTOM_PARAMS)
+        )
+        # TODO: Implement asking params from user.
+
+        # params = CameraParameters(8, 8, 8, 1, True, 200, 1000)
+        # data.extend(params.model_dump_json().encode())
+        raise NotImplementedError()
+    else:
+        raise ValueError("unknown camera action {}", cmd_path[0])
+    return q.add_pus_tc(
+        PusTc(service=8, subservice=128, apid=EXPERIMENT_APID, app_data=data)
+    )

pytmtc/opssat_tmtc/controller.py

@@ -0,0 +1,53 @@
+import enum
+from typing import List
+from spacepackets.ecss import PusTc
+from tmtccmd.config import CmdTreeNode
+from tmtccmd.tmtc import DefaultPusQueueHelper
+
+from opssat_tmtc.common import EXPERIMENT_APID, UniqueId, make_action_cmd_header
+
+
+class ActionId(enum.IntEnum):
+    STOP_EXPERIMENT = 1
+    DOWNLINK_LOG_FILE = 2
+    DOWNLINK_IMAGES_BY_MOVING = 3
+    EXECUTE_SHELL_CMD_BLOCKING = 4
+
+
+class OpCode:
+    DOWNLINK_LOGS = "downlink_logs"
+    DOWNLINK_IMAGES_BY_MOVING = "move_image_files"
+
+
+def create_controller_node():
+    controller_node = CmdTreeNode("controller", "Main OBSW Controller")
+    controller_node.add_child(
+        CmdTreeNode(OpCode.DOWNLINK_LOGS, "Downlink Logs via toGround folder")
+    )
+    controller_node.add_child(
+        CmdTreeNode(
+            OpCode.DOWNLINK_IMAGES_BY_MOVING,
+            "Downlink all image files via the toGroundLP folder",
+        )
+    )
+    return controller_node
+
+
+def create_ctrl_cmd(q: DefaultPusQueueHelper, cmd_path: List[str]):
+    assert len(cmd_path) >= 1
+    data = bytearray()
+    if cmd_path[0] == OpCode.DOWNLINK_LOGS:
+        data.extend(
+            make_action_cmd_header(UniqueId.Controller, ActionId.DOWNLINK_LOG_FILE)
+        )
+    elif cmd_path[0] == OpCode.DOWNLINK_IMAGES_BY_MOVING:
+        data.extend(
+            make_action_cmd_header(
+                UniqueId.Controller, ActionId.DOWNLINK_IMAGES_BY_MOVING
+            )
+        )
+    else:
+        raise ValueError("unknown controller action {}", cmd_path[0])
+    return q.add_pus_tc(
+        PusTc(service=8, subservice=128, apid=EXPERIMENT_APID, app_data=data)
+    )

pytmtc/opssat_tmtc/pus_tc.py

@@ -10,12 +10,8 @@ from tmtccmd.tmtc import DefaultPusQueueHelper
 from tmtccmd.pus.s11_tc_sched import create_time_tagged_cmd
 from tmtccmd.pus.s200_fsfw_mode import Subservice as ModeSubservice
 
-from opssat_tmtc.camera import CameraParameters
+from opssat_tmtc.camera import create_cam_cmd, create_camera_node
-from opssat_tmtc.common import (
+from opssat_tmtc.controller import create_controller_node, create_ctrl_cmd
-    EXPERIMENT_APID,
-    UniqueId,
-    make_action_cmd_header,
-)
 
 
 _LOGGER = logging.getLogger(__name__)
@@ -71,42 +67,8 @@ def create_cmd_definition_tree() -> CmdTreeNode:
     )
     root_node.add_child(scheduler_node)
 
-    action_node = CmdTreeNode("action", "Action Node")
+    root_node.add_child(create_camera_node())
-    cam_node = CmdTreeNode("take_image", "Take Image with IMS Imager")
+    root_node.add_child(create_controller_node())
-    cam_node.add_child(
-        CmdTreeNode("default_single", "Default Single Image Camera Parameters")
-    )
-    cam_node.add_child(
-        CmdTreeNode("balanced_single", "Balanced Single Image Camera Parameters")
-    )
-    cam_node.add_child(
-        CmdTreeNode(
-            "default_single_flatsat",
-            "Default Single Image Camera Parameters for use on FlatSat",
-        )
-    )
-    cam_node.add_child(
-        CmdTreeNode(
-            "balanced_single_flatsat",
-            "Balanced Single Image Camera Parameters for use on FlatSat",
-        )
-    )
-    cam_node.add_child(
-        CmdTreeNode("custom_params", "Custom Camera Parameters as specified from file")
-    )
-    action_node.add_child(cam_node)
-
-    controller_node = CmdTreeNode("controller", "Main OBSW Controller")
-    controller_node.add_child(
-        CmdTreeNode("downlink_logs", "Downlink Logs via toGround folder")
-    )
-    controller_node.add_child(
-        CmdTreeNode("downlink_last_img", "Downlink last image via toGroundLP folder")
-    )
-    action_node.add_child(controller_node)
-
-    root_node.add_child(action_node)
-
     return root_node
 
 
@@ -139,45 +101,10 @@ def pack_pus_telecommands(q: DefaultPusQueueHelper, cmd_path: str):
             )
     if cmd_path_list[0] == "acs":
         assert len(cmd_path_list) >= 2
-    if cmd_path_list[0] == "action":
+    if cmd_path_list[0] == "cam":
-        assert len(cmd_path_list) >= 2
+        create_cam_cmd(q, cmd_path_list[1:])
-        if cmd_path_list[1] == "take_image":
+    if cmd_path_list[0] == "controller":
-            assert len(cmd_path_list) >= 3
+        create_ctrl_cmd(q, cmd_path_list[1:])
-            q.add_log_cmd(
-                "Sending PUS take image action request with "
-                + cmd_path_list[2]
-                + " params."
-            )
-            data = bytearray()
-            if cmd_path_list[2] == "default_single":
-                data.extend(make_action_cmd_header(UniqueId.CameraHandler, 1))
-            if cmd_path_list[2] == "balanced_single":
-                data.extend(make_action_cmd_header(UniqueId.CameraHandler, 2))
-            if cmd_path_list[2] == "default_single_flatsat":
-                data.extend(make_action_cmd_header(UniqueId.CameraHandler, 3))
-            if cmd_path_list[2] == "balanced_single_flatsat":
-                data.extend(make_action_cmd_header(UniqueId.CameraHandler, 4))
-            if cmd_path_list[2] == "custom":
-                data.extend(make_action_cmd_header(UniqueId.CameraHandler, 5))
-                params = CameraParameters(8, 8, 8, 1, True, 200, 1000)
-                data.extend(params.serialize_for_uplink())
-            return q.add_pus_tc(
-                PusTelecommand(
-                    service=8, subservice=128, apid=EXPERIMENT_APID, app_data=data
-                )
-            )
-        if cmd_path_list[1] == "controller":
-            assert len(cmd_path_list) >= 3
-            data = bytearray()
-            if cmd_path_list[2] == "downlink_logs":
-                data.extend(make_action_cmd_header(UniqueId.Controller, 2))
-            if cmd_path_list[2] == "downlink_last_img":
-                data.extend(make_action_cmd_header(UniqueId.Controller, 3))
-            return q.add_pus_tc(
-                PusTelecommand(
-                    service=8, subservice=128, apid=EXPERIMENT_APID, app_data=data
-                )
-            )
 
 
 def handle_set_mode_cmd(

src/config.rs

@@ -283,6 +283,8 @@ pub mod ctrl_err {
     pub const IMAGE_NOT_FOUND_FOR_COPY: ResultU16 = ResultU16::new(GroupId::Controller as u8, 5);
     #[resultcode]
     pub const INVALID_LOGFILE_PATH: ResultU16 = ResultU16::new(GroupId::Controller as u8, 6);
+    #[resultcode]
+    pub const IO_ERROR: ResultU16 = ResultU16::new(GroupId::Controller as u8, 7);
 
     pub const CTRL_ERR_RESULTS: &[ResultU16Info] = &[
         INVALID_CMD_FORMAT_EXT,
@@ -292,6 +294,7 @@ pub mod ctrl_err {
         FILESYSTEM_COPY_ERROR_EXT,
         IMAGE_NOT_FOUND_FOR_COPY_EXT,
         INVALID_LOGFILE_PATH_EXT,
+        IO_ERROR_EXT,
     ];
 }
 

src/controller.rs

@@ -14,6 +14,7 @@ use satrs::{
 };
 use serde::{Deserialize, Serialize};
 use std::env::temp_dir;
+use std::io;
 use std::{
     path::{Path, PathBuf},
     process::Command,
@@ -21,8 +22,8 @@ use std::{
 };
 
 use ops_sat_rs::config::ctrl_err::{
-    FILESYSTEM_COPY_ERROR, IMAGE_NOT_FOUND_FOR_COPY, INVALID_LOGFILE_PATH,
+    FILESYSTEM_COPY_ERROR, INVALID_LOGFILE_PATH, IO_ERROR, SHELL_CMD_EXECUTION_FAILURE,
-    SHELL_CMD_EXECUTION_FAILURE, SHELL_CMD_INVALID_FORMAT, SHELL_CMD_IO_ERROR,
+    SHELL_CMD_INVALID_FORMAT, SHELL_CMD_IO_ERROR,
 };
 
 use crate::requests::CompositeRequest;
@@ -38,7 +39,9 @@ pub struct ShellCmd<'a> {
 pub enum ActionId {
     StopExperiment = 1,
     DownlinkLogfile = 2,
-    DownlinkImages = 3,
+    /// Standard command to download the images made by the camera. It moves all image related
+    /// files inside the home folder into the toGroundLP (low priority to ground download) folder.
+    DownlinkImagesByMoving = 3,
     ExecuteShellCommandBlocking = 4,
 }
 
@@ -140,41 +143,29 @@ impl ExperimentController {
                 self.handle_shell_command_execution(&requestor, &action_req);
             }
             ActionId::DownlinkLogfile => self.handle_downlink_logfile(&requestor, &action_req),
-            // downlink images, default will be the last image, otherwise specified counting down (2 = second to last image, etc.)
+            ActionId::DownlinkImagesByMoving => {
-            ActionId::DownlinkImages => self.handle_downlink_cam_image(&requestor, &action_req),
+                let result = self.handle_downlink_cam_image_by_moving(&requestor, &action_req);
+                if let Err(e) = result {
+                    send_completion_failure(IO_ERROR, Some(e.to_string().into()));
+                }
+            }
         }
     }
 
-    pub fn handle_downlink_cam_image(
+    pub fn handle_downlink_cam_image_by_moving(
         &self,
         requestor: &MessageMetadata,
         action_req: &ActionRequest,
-    ) {
+    ) -> io::Result<()> {
-        log::info!("copying images into low priority downlink folder");
+        log::info!("moving images into low priority downlink folder");
-        let image_path_result = match &action_req.variant {
+        let num_moved_files = move_images_inside_home_dir_to_low_prio_ground_dir(
-            ActionRequestVariant::VecData(data) => {
+            &HOME_PATH,
-                let index = data[0];
+            &self.paths.to_ground_low_prio_dir,
-                get_latest_image(index as usize)
+        )?;
-            }
+        log::info!("moved {} image files", num_moved_files);
-            _ => get_latest_image(0),
+        // TODO: Trigger event containing the number of moved files?
-        };
+        self.send_completion_success(requestor, action_req);
-        match image_path_result {
+        Ok(())
-            Ok(image_path) => self.handle_file_copy(
-                requestor,
-                action_req,
-                &image_path,
-                &self.paths.to_ground_dir,
-            ),
-            Err(e) => {
-                log::warn!("could not retrieve image path: {}", e);
-                self.send_completion_failure(
-                    requestor,
-                    action_req,
-                    IMAGE_NOT_FOUND_FOR_COPY,
-                    Some(e.to_string().into()),
-                );
-            }
-        }
     }
 
     pub fn handle_downlink_logfile(&self, requestor: &MessageMetadata, action_req: &ActionRequest) {
@@ -318,7 +309,34 @@ impl ExperimentController {
     }
 }
 
+pub fn move_images_inside_home_dir_to_low_prio_ground_dir(
+    home_dir: &Path,
+    low_prio_target_dir: &Path,
+) -> io::Result<u32> {
+    let mut moved_files = 0;
+    for dir_entry_result in std::fs::read_dir(home_dir)? {
+        if let Ok(dir_entry) = &dir_entry_result {
+            if let Ok(file_type) = dir_entry.file_type() {
+                if file_type.is_file() {
+                    let path_name = dir_entry.file_name();
+                    let path_name_str = path_name.to_string_lossy();
+                    if path_name_str.contains("img_msec_") {
+                        let mut target_path = PathBuf::new();
+                        target_path.push(low_prio_target_dir);
+                        target_path.push(&path_name);
+                        log::info!("moving file {}", &path_name_str);
+                        std::fs::rename(dir_entry.path(), target_path)?;
+                        moved_files += 1;
+                    }
+                }
+            }
+        }
+    }
+    Ok(moved_files)
+}
+
 // TODO no idea if this works in any way shape or form
+#[allow(dead_code)]
 pub fn get_latest_image(index: usize) -> Result<PathBuf, std::io::Error> {
     // Get the most recently modified file
     let mut png_files = std::fs::read_dir(HOME_FOLDER_EXPERIMENT)?

src/handlers/camera.rs

@@ -230,6 +230,7 @@ impl Ims100BatchHandler {
                 {
                     log::error!("sending data reply unexpectedly failed: {e}");
                 }
+                self.create_metadata_file(&param);
             }
             Err(e) => match e {
                 CameraError::TakeImageError(ref err_str) => {
@@ -261,6 +262,10 @@ impl Ims100BatchHandler {
         }
     }
 
+    pub fn create_metadata_file(&mut self, _param: &CameraPictureParameters) {
+        // TODO: Implement
+    }
+
     pub fn send_completion_success(&self, requestor: &MessageMetadata, action_req: &ActionRequest) {
         let result = self.action_reply_tx.send(GenericMessage::new_action_reply(
             *requestor,

src/main.rs

@@ -154,14 +154,20 @@ fn main() {
     );
 
     let sock_addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
-    let udp_tc_server = UdpTcServer::new(UDP_SERVER.id(), sock_addr, 2048, tc_source_tx.clone())
+    let udp_tc_server_result =
-        .expect("creating UDP TMTC server failed");
+        UdpTcServer::new(UDP_SERVER.id(), sock_addr, 2048, tc_source_tx.clone());
-    let mut udp_tmtc_server = UdpTmtcServer {
+    if udp_tc_server_result.is_err() {
+        log::error!("UDP server creation failed");
+    }
+    let mut opt_udp_tmtc_server = None;
+    if let Ok(udp_tc_server) = udp_tc_server_result {
+        opt_udp_tmtc_server = Some(UdpTmtcServer {
             udp_tc_server,
             tm_handler: DynamicUdpTmHandler {
                 tm_rx: tm_tcp_server_rx,
             },
-    };
+        });
+    }
 
     let tcp_server_cfg = ServerConfig::new(
         TCP_SERVER.id(),
@@ -243,7 +249,9 @@ fn main() {
         .spawn(move || {
             info!("Running UDP server on port {SERVER_PORT}");
             loop {
+                if let Some(ref mut udp_tmtc_server) = opt_udp_tmtc_server {
                     udp_tmtc_server.periodic_operation();
+                }
                 tmtc_task.periodic_operation();
                 if tmtc_stop_signal.load(std::sync::atomic::Ordering::Relaxed) {
                     break;
@@ -307,10 +315,14 @@ fn main() {
         .unwrap();
 
     info!("Starting event handling task");
+    let event_stop_signal = stop_signal.clone();
     let jh_event_handling = thread::Builder::new()
         .name("sat-rs events".to_string())
         .spawn(move || loop {
             event_handler.periodic_operation();
+            if event_stop_signal.load(std::sync::atomic::Ordering::Relaxed) {
+                break;
+            }
             thread::sleep(Duration::from_millis(FREQ_MS_EVENT_HANDLING));
         })
         .unwrap();

src/pus/action.rs

@@ -252,24 +252,29 @@ impl TargetedPusService for ActionServiceWrapper {
     fn poll_and_handle_next_tc(&mut self, time_stamp: &[u8]) -> HandlingStatus {
         match self.service.poll_and_handle_next_tc(time_stamp) {
             Ok(result) => match result {
-                PusPacketHandlerResult::RequestHandled => {}
+                PusPacketHandlerResult::RequestHandled => {
+                    return HandlingStatus::HandledOne;
+                }
                 PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
-                    warn!("PUS 8 partial packet handling success: {e:?}")
+                    warn!("PUS 8 partial packet handling success: {e:?}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::CustomSubservice(invalid, _) => {
                     warn!("PUS 8 invalid subservice {invalid}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
                     warn!("PUS 8 subservice {subservice} not implemented");
+                    return HandlingStatus::HandledOne;
                 }
-                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+                PusPacketHandlerResult::Empty => (),
             },
             Err(error) => {
                 error!("PUS packet handling error: {error:?}");
-                return HandlingStatus::Empty;
             }
         }
-        HandlingStatus::HandledOne
+        // To avoid permanent loops, treat queue empty by default (all tasks done).
+        HandlingStatus::Empty
     }
 
     fn poll_and_handle_next_reply(&mut self, time_stamp: &[u8]) -> HandlingStatus {

src/pus/event.rs

@@ -45,22 +45,28 @@ impl EventServiceWrapper {
     pub fn poll_and_handle_next_tc(&mut self, time_stamp: &[u8]) -> HandlingStatus {
         match self.handler.poll_and_handle_next_tc(time_stamp) {
             Ok(result) => match result {
-                PusPacketHandlerResult::RequestHandled => {}
+                PusPacketHandlerResult::RequestHandled => {
+                    return HandlingStatus::HandledOne;
+                }
                 PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
-                    warn!("PUS 5 partial packet handling success: {e:?}")
+                    warn!("PUS 5 partial packet handling success: {e:?}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::CustomSubservice(invalid, _) => {
                     warn!("PUS 5 invalid subservice {invalid}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
                     warn!("PUS 5 subservice {subservice} not implemented");
+                    return HandlingStatus::HandledOne;
                 }
-                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+                PusPacketHandlerResult::Empty => (),
             },
             Err(error) => {
-                error!("PUS packet handling error: {error:?}")
+                error!("PUS packet handling error: {error:?}");
             }
         }
-        HandlingStatus::HandledOne
+        // To avoid permanent loops, treat queue empty by default (all tasks done).
+        HandlingStatus::Empty
     }
 }

src/pus/hk.rs

@@ -271,25 +271,29 @@ impl HkServiceWrapper {
     pub fn poll_and_handle_next_tc(&mut self, time_stamp: &[u8]) -> HandlingStatus {
         match self.service.poll_and_handle_next_tc(time_stamp) {
             Ok(result) => match result {
-                PusPacketHandlerResult::RequestHandled => {}
+                PusPacketHandlerResult::RequestHandled => {
+                    return HandlingStatus::HandledOne;
+                }
                 PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
-                    warn!("PUS 3 partial packet handling success: {e:?}")
+                    warn!("PUS 3 partial packet handling success: {e:?}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::CustomSubservice(invalid, _) => {
                     warn!("PUS 3 invalid subservice {invalid}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
                     warn!("PUS 3 subservice {subservice} not implemented");
+                    return HandlingStatus::HandledOne;
                 }
-                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+                PusPacketHandlerResult::Empty => (),
             },
             Err(error) => {
                 error!("PUS packet handling error: {error:?}");
-                // To avoid permanent loops on error cases.
-                return HandlingStatus::Empty;
             }
         }
-        HandlingStatus::HandledOne
+        // To avoid permanent loops, treat queue empty by default (all tasks done).
+        HandlingStatus::Empty
     }
 
     pub fn poll_and_handle_next_reply(&mut self, time_stamp: &[u8]) -> HandlingStatus {

src/pus/mode.rs

@@ -243,25 +243,29 @@ impl TargetedPusService for ModeServiceWrapper {
     fn poll_and_handle_next_tc(&mut self, time_stamp: &[u8]) -> HandlingStatus {
         match self.service.poll_and_handle_next_tc(time_stamp) {
             Ok(result) => match result {
-                PusPacketHandlerResult::RequestHandled => {}
+                PusPacketHandlerResult::RequestHandled => {
+                    return HandlingStatus::HandledOne;
+                }
                 PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
-                    warn!("PUS mode service: partial packet handling success: {e:?}")
+                    warn!("PUS mode service: partial packet handling success: {e:?}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::CustomSubservice(invalid, _) => {
                     warn!("PUS mode service: invalid subservice {invalid}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
                     warn!("PUS mode service: {subservice} not implemented");
+                    return HandlingStatus::HandledOne;
                 }
-                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+                PusPacketHandlerResult::Empty => (),
             },
             Err(error) => {
                 error!("PUS mode service: packet handling error: {error:?}");
-                // To avoid permanent loops on error cases.
-                return HandlingStatus::Empty;
             }
         }
-        HandlingStatus::HandledOne
+        // To avoid permanent loops, treat queue empty by default (all tasks done).
+        HandlingStatus::Empty
     }
 
     fn poll_and_handle_next_reply(&mut self, time_stamp: &[u8]) -> HandlingStatus {

src/pus/scheduler.rs

@@ -106,23 +106,29 @@ impl SchedulingService {
             .poll_and_handle_next_tc(time_stamp, &mut self.sched_tc_pool)
         {
             Ok(result) => match result {
-                PusPacketHandlerResult::RequestHandled => {}
+                PusPacketHandlerResult::RequestHandled => {
+                    return HandlingStatus::HandledOne;
+                }
                 PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
-                    warn!("PUS11 partial packet handling success: {e:?}")
+                    warn!("PUS11 partial packet handling success: {e:?}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::CustomSubservice(invalid, _) => {
                     warn!("PUS11 invalid subservice {invalid}");
+                    return HandlingStatus::HandledOne;
                 }
                 PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
                     warn!("PUS11: Subservice {subservice} not implemented");
+                    return HandlingStatus::HandledOne;
                 }
-                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+                PusPacketHandlerResult::Empty => (),
             },
             Err(error) => {
                 error!("PUS packet handling error: {error:?}")
             }
         }
-        HandlingStatus::HandledOne
+        // To avoid permanent loops, treat queue empty by default (all tasks done).
+        HandlingStatus::Empty
     }
 }
 

src/pus/stack.rs

@@ -27,6 +27,7 @@ impl PusStack {
             .expect("time stamp generation error")
             .to_vec()
             .unwrap();
+        let mut loop_count = 0;
         loop {
             let mut nothing_to_do = true;
             let mut is_srv_finished =
@@ -69,6 +70,12 @@ impl PusStack {
                 self.mode_srv.poll_and_handle_next_tc(&time_stamp),
                 Some(self.mode_srv.poll_and_handle_next_reply(&time_stamp)),
             );
+            // Safety mechanism to avoid infinite loops.
+            loop_count += 1;
+            if loop_count >= 500 {
+                log::warn!("reached PUS stack loop count 500, breaking");
+                break;
+            }
             if nothing_to_do {
                 // Timeout checking is only done once.
                 self.action_srv_wrapper.check_for_request_timeouts();

src/pus/test.rs

@@ -57,15 +57,18 @@ impl TestCustomServiceWrapper {
             PusPacketHandlerResult::RequestHandled => {
                 info!("Received PUS ping command TC[17,1]");
                 info!("Sent ping reply PUS TM[17,2]");
+                return HandlingStatus::HandledOne;
             }
             PusPacketHandlerResult::RequestHandledPartialSuccess(partial_err) => {
                 warn!(
                     "Handled PUS ping command with partial success: {:?}",
                     partial_err
                 );
+                return HandlingStatus::HandledOne;
             }
             PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
-                warn!("PUS17: Subservice {subservice} not implemented")
+                warn!("PUS17: Subservice {subservice} not implemented");
+                return HandlingStatus::HandledOne;
             }
             // TODO: adapt interface events are implemented
             PusPacketHandlerResult::CustomSubservice(subservice, token) => {
@@ -115,9 +118,11 @@ impl TestCustomServiceWrapper {
                         )
                         .expect("Sending start failure verification failed");
                 }
+                return HandlingStatus::HandledOne;
             }
-            PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
+            PusPacketHandlerResult::Empty => (),
         }
-        HandlingStatus::HandledOne
+        // To avoid permanent loops, treat queue empty by default (all tasks done).
+        HandlingStatus::Empty
     }
 }