diff --git a/pytmtc/opssat_tmtc/camera.py b/pytmtc/opssat_tmtc/camera.py index e20d0b3..21dced9 100644 --- a/pytmtc/opssat_tmtc/camera.py +++ b/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) + ) diff --git a/pytmtc/opssat_tmtc/controller.py b/pytmtc/opssat_tmtc/controller.py new file mode 100644 index 0000000..a4481e1 --- /dev/null +++ b/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) + ) diff --git a/pytmtc/opssat_tmtc/pus_tc.py b/pytmtc/opssat_tmtc/pus_tc.py index e368e01..e50b536 100644 --- a/pytmtc/opssat_tmtc/pus_tc.py +++ b/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.common import ( - EXPERIMENT_APID, - UniqueId, - make_action_cmd_header, -) +from opssat_tmtc.camera import create_cam_cmd, create_camera_node +from opssat_tmtc.controller import create_controller_node, create_ctrl_cmd _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") - cam_node = CmdTreeNode("take_image", "Take Image with IMS Imager") - 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) - + root_node.add_child(create_camera_node()) + root_node.add_child(create_controller_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": - assert len(cmd_path_list) >= 2 - if cmd_path_list[1] == "take_image": - assert len(cmd_path_list) >= 3 - 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 - ) - ) + if cmd_path_list[0] == "cam": + create_cam_cmd(q, cmd_path_list[1:]) + if cmd_path_list[0] == "controller": + create_ctrl_cmd(q, cmd_path_list[1:]) def handle_set_mode_cmd( diff --git a/src/controller.rs b/src/controller.rs index 88a62b6..667cd09 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -323,17 +323,11 @@ pub fn move_images_inside_home_dir_to_low_prio_ground_dir( 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 image file from {:?} to {:?}", - dir_entry, - target_path - ); + target_path.push(&path_name); + log::info!("moving file {}", &path_name_str); std::fs::rename(dir_entry.path(), target_path)?; moved_files += 1; } - - log::info!("found image file: {:?}", dir_entry); } } } diff --git a/src/main.rs b/src/main.rs index 2396d90..7e84a46 100644 --- a/src/main.rs +++ b/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()) - .expect("creating UDP TMTC server failed"); - let mut udp_tmtc_server = UdpTmtcServer { - udp_tc_server, - tm_handler: DynamicUdpTmHandler { - tm_rx: tm_tcp_server_rx, - }, - }; + let udp_tc_server_result = + UdpTcServer::new(UDP_SERVER.id(), sock_addr, 2048, tc_source_tx.clone()); + 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 { - udp_tmtc_server.periodic_operation(); + 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(); diff --git a/src/pus/action.rs b/src/pus/action.rs index 845bcfe..5b270c2 100644 --- a/src/pus/action.rs +++ b/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 { diff --git a/src/pus/event.rs b/src/pus/event.rs index 4bab09a..aab9081 100644 --- a/src/pus/event.rs +++ b/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 } } diff --git a/src/pus/hk.rs b/src/pus/hk.rs index 84e6b37..a005df6 100644 --- a/src/pus/hk.rs +++ b/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 { diff --git a/src/pus/mode.rs b/src/pus/mode.rs index 2007d1f..90f822c 100644 --- a/src/pus/mode.rs +++ b/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 { diff --git a/src/pus/scheduler.rs b/src/pus/scheduler.rs index 5e696a9..c4d6227 100644 --- a/src/pus/scheduler.rs +++ b/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 } } diff --git a/src/pus/stack.rs b/src/pus/stack.rs index fcc06e6..cbed611 100644 --- a/src/pus/stack.rs +++ b/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(); diff --git a/src/pus/test.rs b/src/pus/test.rs index 31695c2..4c55d16 100644 --- a/src/pus/test.rs +++ b/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 } }