From 47e36cb0b5ffd712531768facdcc9d6176483b17 Mon Sep 17 00:00:00 2001 From: lkoester Date: Fri, 20 Jan 2023 14:27:44 +0100 Subject: [PATCH] added pus service 8 to obsw and pyclient --- pyclient/main.py | 75 ++++++++++++++++++++++++++--------- pyclient/tmtc_conf.json | 2 +- src/action.rs | 21 ++++++++++ src/device_handler.rs | 5 ++- src/main.rs | 87 +++++++++++++++++++++++++++++++++++++---- src/pus.rs | 47 +++++++++++++++++++++- src/requests.rs | 3 +- 7 files changed, 208 insertions(+), 32 deletions(-) create mode 100644 src/action.rs diff --git a/pyclient/main.py b/pyclient/main.py index c06f9ed..e57c5fd 100644 --- a/pyclient/main.py +++ b/pyclient/main.py @@ -4,7 +4,7 @@ import enum import struct import sys import time -from typing import Optional +from typing import Optional, Tuple import tmtccmd from spacepackets.ecss import PusTelemetry, PusTelecommand, PusVerificator @@ -12,7 +12,7 @@ from spacepackets.ecss.pus_17_test import Service17Tm from spacepackets.ecss.pus_1_verification import UnpackParams, Service1Tm from tmtccmd import CcsdsTmtcBackend, TcHandlerBase, ProcedureParamsWrapper -from tmtccmd.tc.pus_3_fsfw_hk import generate_one_hk_command, make_sid +from tmtccmd.tc.pus_3_fsfw_hk import generate_one_hk_command, make_sid, Subservices from tmtccmd.core.base import BackendRequest from tmtccmd.pus import VerificationWrapper from tmtccmd.tm import CcsdsTmHandler, SpecificApidHandlerBase @@ -52,6 +52,10 @@ LOGGER = get_console_logger() EXAMPLE_PUS_APID = 0x02 +class ServiceList: + AOCS = "aocs" + + class SatRsConfigHook(TmTcCfgHookBase): def __init__(self, json_cfg_path: str): super().__init__(json_cfg_path=json_cfg_path) @@ -87,13 +91,23 @@ class SatRsConfigHook(TmTcCfgHookBase): info="PUS Service 17 Test", op_code_entry=srv_17, ) - srv_3 = OpCodeEntry() - srv_3.add(HkOpCodes.GENERATE_ONE_SHOT, "Generate AOCS one shot HK") + + srv_8 = OpCodeEntry() + srv_8.add("1", "Camera Image Request") defs.add_service( - name=CoreServiceList.SERVICE_3, - info="PUS Service 3 Housekeeping", - op_code_entry=srv_3 + name=CoreServiceList.SERVICE_8, + info="PUS Service 8 Action", + op_code_entry=srv_8, ) + + oce = OpCodeEntry() + oce.add(AocsOpCodes.ONE_SHOT, "Generate AOCS one shot HK") + defs.add_service( + name=ServiceList.AOCS, + info="AOCS service", + op_code_entry=oce + ) + return defs def perform_mode_operation(self, tmtc_backend: CcsdsTmtcBackend, mode: int): @@ -143,13 +157,21 @@ class PusHandler(SpecificApidHandlerBase): self.verif_wrapper.log_to_file(tm_packet, res) dedicated_handler = True if service == 3: - LOGGER.info("No handling for HK packets implemented") - LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]") pus_tm = PusTelemetry.unpack(packet) - if pus_tm.subservice == 25: - if len(pus_tm.source_data) < 8: - raise ValueError("No addressable ID in HK packet") - json_str = pus_tm.source_data[8:] + if pus_tm.subservice == Subservices.TM_HK_REPORT: + target_id, set_id = read_addressable_id(pus_tm.source_data) + known_hk = False + if target_id == RequestTargetId.ACS: + if set_id == AcsHkIds.GOD_SET: + known_hk = True + LOGGER.info(pus_tm.source_data[8:]) + if not known_hk: + LOGGER.info("No handling for HK packets implemented") + LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]") + if pus_tm.subservice == 25: + if len(pus_tm.source_data) < 8: + raise ValueError("No addressable ID in HK packet") + json_str = pus_tm.source_data[8:] dedicated_handler = True if service == 5: tm_packet = Service5Tm.unpack(packet) @@ -181,17 +203,27 @@ def make_addressable_id(target_id: int, unique_id: int) -> bytes: byte_string.extend(struct.pack("!I", unique_id)) return byte_string +def make_target_id(target_id: int) -> bytes: + byte_string = bytearray(struct.pack("!I", target_id)) + return byte_string + +def read_addressable_id(data: bytes) -> Tuple[int, int]: + target_id = struct.unpack("!I", data[0:4])[0] + set_id = struct.unpack("!I", data[4:8])[0] + return (target_id, set_id) + class RequestTargetId(enum.IntEnum): ACS = 1 + PLD = 2 class AcsHkIds(enum.IntEnum): - MGM_SET = 1 + GOD_SET = 0 -class HkOpCodes: - GENERATE_ONE_SHOT = ["0", "oneshot"] +class AocsOpCodes: + ONE_SHOT = ["hk_oneshot"] class TcHandler(TcHandlerBase): @@ -247,11 +279,16 @@ class TcHandler(TcHandlerBase): return q.add_pus_tc( PusTelecommand(service=17, subservice=1) ) - if service == CoreServiceList.SERVICE_3: - if op_code in HkOpCodes.GENERATE_ONE_SHOT: + if service == CoreServiceList.SERVICE_8: + q.add_log_cmd("Sending PUS action request telecommand") + return q.add_pus_tc( + PusTelecommand(service=8, subservice=1, app_data=make_target_id(RequestTargetId.PLD)) + ) + if service == ServiceList.AOCS: + if op_code in AocsOpCodes.ONE_SHOT: q.add_log_cmd("Sending HK one shot request") q.add_pus_tc( - generate_one_hk_command(make_addressable_id(RequestTargetId.ACS, AcsHkIds.MGM_SET)) + generate_one_hk_command(make_addressable_id(RequestTargetId.ACS, AcsHkIds.GOD_SET)) ) pass diff --git a/pyclient/tmtc_conf.json b/pyclient/tmtc_conf.json index ab02100..1a79d49 100644 --- a/pyclient/tmtc_conf.json +++ b/pyclient/tmtc_conf.json @@ -1,6 +1,6 @@ { "com_if": "udp", - "tcpip_udp_ip_addr": "127.0.0.1", + "tcpip_udp_ip_addr": "169.254.7.6", "tcpip_udp_port": 7301, "tcpip_udp_recv_max_size": 1500 } \ No newline at end of file diff --git a/src/action.rs b/src/action.rs new file mode 100644 index 0000000..59f7f97 --- /dev/null +++ b/src/action.rs @@ -0,0 +1,21 @@ +use satrs_core::tmtc::AddressableId; +use eurosim_obsw::RequestTargetId; + +pub type CollectionIntervalFactor = u32; + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum ActionIds { + ImageRequest = 1, +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum ActionRequest { + ImageRequest(RequestTargetId), + OrientationRequest(RequestTargetId), +} + +pub enum Subservice { + ImageRequest = 1, + OrientationRequest = 2, + PointingRequest = 3, +} \ No newline at end of file diff --git a/src/device_handler.rs b/src/device_handler.rs index 8af5cfc..ec2bf69 100644 --- a/src/device_handler.rs +++ b/src/device_handler.rs @@ -1,6 +1,7 @@ use std::collections::HashMap; +use std::hash::Hash; use log::info; -use crate::can_ids::DeviceId; +use crate::can_ids::{DeviceId, PackageId}; use socketcan::{errors, frame, socket, CanFrame, Socket}; pub use num_derive::{FromPrimitive, ToPrimitive}; @@ -16,10 +17,10 @@ pub enum DeviceState { Unknown, } + #[derive(Clone, Debug)] pub struct DeviceHandler { device_state_map: HashMap, - } impl DeviceHandler { diff --git a/src/main.rs b/src/main.rs index 5693721..e507910 100644 --- a/src/main.rs +++ b/src/main.rs @@ -10,6 +10,7 @@ mod requests; mod tmtc; mod aocs; mod cam; +mod action; use crate::hk::{AcsHkIds, HkRequest}; use crate::requests::{Request, RequestWithToken}; @@ -47,10 +48,12 @@ use log::{info, warn}; use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper; use std::collections::HashMap; use std::net::{IpAddr, SocketAddr}; -use std::sync::mpsc::{channel, TryRecvError}; +use std::sync::mpsc::{channel, RecvError, TryRecvError}; use std::sync::{mpsc, Arc, RwLock}; use std::thread; use std::time::Duration; +//use libc::time64_t; +use crate::action::ActionRequest; use crate::cam::CameraRequest; #[derive(Clone)] @@ -140,6 +143,7 @@ fn main() { let pus_event_man_send_provider = MpscEventU32SendProvider::new(1, pus_event_man_tx); let mut reporter_event_handler = verif_reporter.clone(); let mut reporter_aocs = verif_reporter.clone(); + let mut reporter_pld = verif_reporter.clone(); event_man.subscribe_all(pus_event_man_send_provider.id()); let mut request_map = HashMap::new(); @@ -147,6 +151,7 @@ fn main() { let (pld_thread_tx, pld_thread_rx) = channel::(); request_map.insert(RequestTargetId::AcsSubsystem as u32, acs_thread_tx); request_map.insert(RequestTargetId::PldSubsystem as u32, pld_thread_tx); + //add here receivers for tmtc task to send requests to //request_map.insert(RequestTargetId::CanTask as u32, can_thread_tx); let tc_source = PusTcSource { @@ -226,7 +231,7 @@ fn main() { let aocs_tm_funnel_tx = tm_funnel_tx.clone(); let mut aocs_tm_store = tm_store.clone(); - + /* // AOCS Thread let socket1 = can::CanTxHandler::new_socket("can0", ThreadId::AOCSThread, package_map_aocs_tx).unwrap(); @@ -316,6 +321,8 @@ fn main() { } }); + */ + let package_map_pld_tx = load_package_ids(); let pld_tm_funnel_tx = tm_funnel_tx.clone(); let mut pld_tm_store = tm_store.clone(); @@ -325,16 +332,82 @@ fn main() { println!("Starting Payload Handling task"); let builder3 = thread::Builder::new().name("PLDThread".into()); let jh3 = builder3.spawn(move || { + let mut time_stamp_buf: [u8; 7] = [0; 7]; loop { match pld_thread_rx.try_recv() { Ok(request_with_token) => { match request_with_token.0 { - Request::CameraRequest(cam_req) => { - match cam_req { - CameraRequest::OneShot(id) => { + Request::ActionRequest(action_id) => { + match action_id { + ActionRequest::ImageRequest(target_id) => { + assert_eq!(target_id, RequestTargetId::PldSubsystem); + // get current time stamp + let cds_stamp = + TimeProvider::from_now_with_u16_days().unwrap(); + cds_stamp.write_to_bytes(&mut time_stamp_buf); + // send start verification and get token + let start_token = reporter_pld + .start_success(request_with_token.1, &time_stamp_buf) + .expect("Error sending start success."); + + // make can bus package to camera + let data = [1]; + PLDCanSocket.tx_socket(PackageId::CameraImageRequest, &data); + + //let timeout = Duration::from_millis(400); + + // loop to allow early exit incase verif never arrives + + // wait for request verification + loop { + match pld_can_rx.recv() { + Ok(msg) => { + if msg.package_id() == PackageId::CameraImageRequestConfirmation && msg.data()[0] == 1 { + break; + } + } + Err(_) => { + warn!("Error receiving Can Bus Message."); + } + } + } + // wait for start of execution + loop { + match pld_can_rx.recv() { + Ok(msg) => { + if msg.package_id() == PackageId::CameraImageExecutionStart && msg.data()[0] == 1 { + break; + } + } + Err(_) => { + warn!("Error receiving Can Bus Message."); + } + } + } + // wait for end of execution + loop { + match pld_can_rx.recv() { + Ok(msg) => { + if msg.package_id() == PackageId::CameraImageExectutionEnd && msg.data()[0] == 1 { + let cds_stamp = + TimeProvider::from_now_with_u16_days().unwrap(); + cds_stamp.write_to_bytes(&mut time_stamp_buf); + + // send end verification with token + reporter_pld + .completion_success(start_token, &time_stamp_buf) + .expect("Error sending start success."); + break; + } + } + Err(_) => { + warn!("Error receiving Can Bus Message."); + } + } + } } - CameraRequest::OneShotWithTimeStamp(id) => {} + ActionRequest::OrientationRequest(_) => {} } } _ => {} @@ -369,7 +442,7 @@ fn main() { jh1.unwrap() .join() .expect("Joining CAN Bus Listening thread failed"); - jh2.unwrap().join().expect("Joing AOCS thread failed"); + //jh2.unwrap().join().expect("Joing AOCS thread failed"); jh3.unwrap().join().expect("Joing AOCS thread failed"); jh4.unwrap().join().expect("Joing AOCS thread failed"); diff --git a/src/pus.rs b/src/pus.rs index b71de50..8623c01 100644 --- a/src/pus.rs +++ b/src/pus.rs @@ -1,7 +1,7 @@ use crate::hk::{CollectionIntervalFactor, HkRequest}; use crate::requests::{Request, RequestWithToken}; use crate::tmtc::{PusTcSource, TmStore}; -use eurosim_obsw::{hk_err, tmtc_err}; +use eurosim_obsw::{hk_err, RequestTargetId, tmtc_err}; use satrs_core::events::EventU32; use satrs_core::pool::StoreAddr; use satrs_core::pus::event::Subservices; @@ -20,6 +20,10 @@ use satrs_core::{ use std::collections::HashMap; use std::sync::mpsc::Sender; +use satrs_core::spacepackets::ecss::PusServiceId::Action; +use eurosim_obsw::RequestTargetId::{AcsSubsystem, PldSubsystem}; +use crate::action; +use crate::action::ActionRequest; pub struct PusReceiver { pub tm_helper: PusTmWithCdsShortHelper, @@ -273,7 +277,46 @@ impl PusReceiver { } } - fn handle_function_request(&mut self, pus_tc: &PusTc, token: VerificationToken) { + fn handle_function_request(&mut self, pus_tc: &PusTc, token: VerificationToken) { + if pus_tc.user_data().is_none() { + self.update_time_stamp(); + /*self.verif_reporter + .start_failure( + token, + FailParams::new(&self.time_stamp, &tmtc_err::NOT_ENOUGH_APP_DATA, None), + ) + .expect("Sending start failure TM failed"); + + */ + return; + } + + + + let send_request = |request: ActionRequest| { + match request { + ActionRequest::ImageRequest(target_id) => { + let id = target_id as u32; + let sender = self.request_map.get(&id).unwrap(); + sender + .send(RequestWithToken(Request::ActionRequest(request), token)) + .unwrap_or_else(|_| panic!("Sending Action request {:?} failed", request)); + } + ActionRequest::OrientationRequest(target_id) => { + let id = target_id as u32; + let sender = self.request_map.get(&id).unwrap(); + sender + .send(RequestWithToken(Request::ActionRequest(request), token)) + .unwrap_or_else(|_| panic!("Sending Action request {:?} failed", request)); + } + } + }; + + if PusPacket::subservice(pus_tc) == action::Subservice::ImageRequest as u8 { + send_request(ActionRequest::ImageRequest(PldSubsystem)); + } else if PusPacket::subservice(pus_tc) == action::Subservice::OrientationRequest as u8 { + send_request(ActionRequest::OrientationRequest(AcsSubsystem)); + } } } diff --git a/src/requests.rs b/src/requests.rs index 7bd0f68..d10f0ac 100644 --- a/src/requests.rs +++ b/src/requests.rs @@ -1,11 +1,12 @@ use crate::hk::HkRequest; use satrs_core::pus::verification::{TcStateAccepted, VerificationToken}; +use crate::action::ActionRequest; use crate::cam::CameraRequest; #[derive(Copy, Clone, Eq, PartialEq, Debug)] pub enum Request { HkRequest(HkRequest), - CameraRequest(CameraRequest), + ActionRequest(ActionRequest), } #[derive(Copy, Clone, Eq, PartialEq, Debug)]