diff --git a/Cargo.lock b/Cargo.lock index 7bc9f71..9f03da8 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -288,6 +288,7 @@ dependencies = [ name = "eurosim-obsw" version = "0.1.0" dependencies = [ + "byteorder", "chrono", "embedded-can", "fern", diff --git a/Cargo.toml b/Cargo.toml index bcbe2fd..1f512d3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -26,6 +26,8 @@ strum_macros = "0.24" num = "0.4" num-derive = "0.3" num-traits = "0.2" +byteorder = "1.4" + [dependencies.socketcan] git = "https://github.com/socketcan-rs/socketcan-rs.git" diff --git a/output.log b/output.log index 675608e..c25705c 100644 --- a/output.log +++ b/output.log @@ -35,3 +35,9 @@ [2023-02-15][14:48:12][main][INFO] Starting Payload Handling task [2023-02-15][14:48:12][main][INFO] Starting TM funnel task [2023-02-15][14:48:12][main][INFO] Starting AOCS task +[2023-02-16][12:46:02][main][INFO] Running DemoSat OBSW! +[2023-02-16][12:46:02][main][INFO] Starting TMTC task +[2023-02-16][12:46:02][main][INFO] Starting power task +[2023-02-16][12:46:02][main][INFO] Starting Payload Handling task +[2023-02-16][12:46:02][main][INFO] Starting TM funnel task +[2023-02-16][12:46:02][main][INFO] Starting AOCS task diff --git a/pyclient/main copy.py b/pyclient/main copy.py index 947f63f..375ea2b 100644 --- a/pyclient/main copy.py +++ b/pyclient/main copy.py @@ -223,6 +223,9 @@ def read_addressable_id(data: bytes) -> tuple[int, int]: return (target_id, set_id) +class CustomServiceList(enum.IntEnum): + ACS = "acs" + class RequestTargetId(enum.IntEnum): ACS = 1 PLD = 2 @@ -291,6 +294,10 @@ class TcHandler(TcHandlerBase): ): q.add_log_cmd("Sending PUS ping telecommand") return q.add_pus_tc(PusTelecommand(service=17, subservice=1)) + if service == CustomServiceList.ACS: + if op_code == "set_mode": + q.add_log_cmd() + q.add_pus_tc() if service == CoreServiceList.SERVICE_11: if op_code == "0": q.add_log_cmd("Sending PUS scheduled TC telecommand") diff --git a/src/aocs_handler.rs b/src/aocs_handler.rs index c226100..5b8a478 100644 --- a/src/aocs_handler.rs +++ b/src/aocs_handler.rs @@ -6,6 +6,8 @@ use crate::requests::{Request, RequestWithToken}; use satrs_core::power::SwitchId; use std::sync::mpsc::{Receiver, Sender}; use serde::{Deserialize, Serialize}; +use byteorder::{LittleEndian, ByteOrder}; +use crate::can_ids::PackageId::AOCSDataRequestMGM1; pub enum AocsSensorMode { Idle, @@ -81,6 +83,7 @@ pub struct MGMHandler { mode_rx: Receiver, action_rx: Receiver, mgm_data: Arc>, + sensor_data_enabled: bool, } impl AocsSensorHandler for MGMHandler { @@ -106,12 +109,12 @@ impl MGMHandler { pub fn new( power_switcher: PowerSwitcher, device_id: DeviceId, - switch_id: SwitchId, can_tx: Sender, can_rx: Receiver, mode_rx: Receiver, action_rx: Receiver, ) -> MGMHandler { + let switch_id = device_id as u16; MGMHandler { power_switcher, device_id, @@ -123,6 +126,7 @@ impl MGMHandler { mode_rx, action_rx, mgm_data: Arc::new(Mutex::new(MGMData::new())), + sensor_data_enabled: false, } } @@ -132,7 +136,8 @@ impl MGMHandler { pub fn periodic_op(&mut self) { self.update_mode(); - //self.handle_action_requests(); + self.handle_requests(); + self.read_sensor_data(); } pub fn update_mode(&mut self) { @@ -160,11 +165,42 @@ impl MGMHandler { pub fn handle_hk_request(&mut self, hk_req: HkRequest) { match hk_req { - HkRequest::OneShot(_) => {} - HkRequest::Enable(_) => {} - HkRequest::Disable(_) => {} + HkRequest::OneShot(_) => { + self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[2]).unwrap()).unwrap(); + } + HkRequest::Enable(_) => { + if !self.sensor_data_enabled { + self.sensor_data_enabled = true; + self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[1]).unwrap()).unwrap(); + } + } + HkRequest::Disable(_) => { + if self.sensor_data_enabled { + self.sensor_data_enabled = false; + self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[0]).unwrap()).unwrap(); + } + } HkRequest::ModifyCollectionInterval(_, _) => {} } } + + pub fn read_sensor_data(&mut self) { + if let Ok(package) = self.can_rx.try_recv() { + let float_data = self.decode_sensor_data(package.data()); + if let Ok(mut mgm_data) = self.mgm_data.lock() { + match package.package_id() { + PackageId::AOCSDataMGM1 => { mgm_data.axis_1 = float_data } + PackageId::AOCSDataMGM2 => { mgm_data.axis_2 = float_data } + PackageId::AOCSDataMGM3 => { mgm_data.axis_3 = float_data } + _ => {} + } + } + + } + } + + pub fn decode_sensor_data(&mut self, buf: &[u8]) -> f64 { + LittleEndian::read_f64(&buf) + } //pub fn handle_action_request(&mut self, action_request: ActionRequest) {} } diff --git a/src/main.rs b/src/main.rs index 50f3b7b..a5614f0 100644 --- a/src/main.rs +++ b/src/main.rs @@ -50,7 +50,7 @@ use std::sync::mpsc::channel; use std::sync::{mpsc, Arc, Mutex, RwLock}; use std::thread; //use libc::time64_t; -use crate::aocs_handler::MGMData; +use crate::aocs_handler::{MGMData, MGMHandler}; #[cfg(feature = "can")] use crate::can::CanTxHandler; use crate::hk::{AocsHousekeeper, AocsSensorData}; @@ -250,12 +250,14 @@ fn main() { let pld_can_tx_sender = can_tx_sender.clone(); + let power_switcher_pld = power_switcher.clone(); + //let mut pcdu_tx_clone = pcdu_tx.clone(); info!("Starting Payload Handling task"); let builder3 = thread::Builder::new().name("PLDThread".into()); let jh3 = builder3.spawn(move || { core_pld_task( - power_switcher.clone(), + power_switcher_pld.clone(), pld_thread_rx, pld_can_rx, pld_can_tx_sender, @@ -287,10 +289,17 @@ fn main() { let mgm_shared_data: Arc> = Arc::default(); let aocs_sensor_data = Arc::new(Mutex::new(AocsSensorData::new())); + let (aocs_mode_tx, aocs_mode_rx) = channel(); + let (mgm_action_tx, mgm_action_rx) = channel(); + + let power_switcher_aocs = power_switcher.clone(); info!("Starting AOCS task"); let builder5 = thread::Builder::new().name("AOCSThread".into()); let jh5 = builder5.spawn(move || { + let mut mgm_handler = MGMHandler::new(power_switcher_aocs.clone(), DeviceId::MGM1, can_tx_sender.clone(), aocs_can_rx, aocs_mode_rx, mgm_action_rx); + + let aocs_sensor_data = Arc::new(Mutex::new(AocsSensorData::new())); let mut aocs_housekeeper = AocsHousekeeper::new( aocs_sensor_data.clone(), aocs_thread_rx, @@ -300,6 +309,10 @@ fn main() { reporter_aocs, ); loop { + mgm_handler.periodic_op(); + let mut locked_sensor_data = aocs_sensor_data.lock().unwrap(); + locked_sensor_data.update_mgm_data(&mgm_handler.get_data_ref()); + drop(locked_sensor_data); aocs_housekeeper.handle_hk_request(); } });