diff --git a/src/aocs_handler.rs b/src/aocs_handler.rs index 829ea0f..d37abf4 100644 --- a/src/aocs_handler.rs +++ b/src/aocs_handler.rs @@ -1,11 +1,13 @@ -use std::sync::mpsc::{Receiver, TryRecvError}; -use satrs_core::power::SwitchId; +use std::sync::{Arc, Mutex}; use crate::can::CanTxHandler; use crate::can_ids::{DeviceId, PackageId, PackageModel}; use crate::hk::HkRequest; use crate::power_handler::{DeviceState, PowerSwitcher}; use crate::requests::{Request, RequestWithToken}; use crate::ActionRequest; +use satrs_core::power::SwitchId; +use std::sync::mpsc::{Receiver, TryRecvError}; +use serde::{Deserialize, Serialize}; pub enum AocsSensorMode { Idle, @@ -32,8 +34,42 @@ pub trait AocsSensorHandler { let id = self.get_package_id()?; self.send_message(id, &[2]) } +} - fn write_to_shared_pool(&mut self) -> Result<(), Self::Error>; +#[derive(Debug, Copy, Clone, Default, Serialize, Deserialize)] +pub struct MGMData { + axis_1: f64, + axis_2: f64, + axis_3: f64, +} + +impl MGMData { + pub fn from_floats(axis_1: f64, + axis_2: f64, + axis_3: f64, + ) -> MGMData { + MGMData{axis_1, axis_2, axis_3} + } + + pub fn new() -> MGMData { MGMData{axis_1: 0.0, axis_2: 0.0, axis_3: 0.0} } + + pub fn update(&mut self, + axis_1: f64, + axis_2: f64, + axis_3: f64 + ) { + self.axis_1 = axis_1; + self.axis_2 = axis_2; + self.axis_3 = axis_3; + } + + pub fn to_array(&self) -> [f64; 3] { + [self.axis_1, self.axis_2, self.axis_3] + } + + pub fn to_tuple(&self) -> (f64, f64, f64) { + (self.axis_1, self.axis_2, self.axis_3) + } } pub struct MGMHandler { @@ -46,6 +82,7 @@ pub struct MGMHandler { mode: AocsSensorMode, mode_rx: Receiver, action_rx: Receiver, + mgm_data: Arc>, } impl AocsSensorHandler for MGMHandler { @@ -57,18 +94,14 @@ impl AocsSensorHandler for MGMHandler { DeviceId::MGM2 => Ok(PackageId::AOCSDataRequestMGM2), DeviceId::MGM3 => Ok(PackageId::AOCSDataRequestMGM3), DeviceId::MGM4 => Ok(PackageId::AOCSDataRequestMGM4), - _ => Err(()) - } + _ => Err(()), + }; } fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error> { self.can_tx.tx_socket(id, buf); return Ok(()); } - - fn write_to_shared_pool(&mut self) -> Result<(), Self::Error> { - todo!() - } } impl MGMHandler { @@ -91,10 +124,15 @@ impl MGMHandler { mode: AocsSensorMode::Idle, mode_rx, action_rx, + mgm_data: Arc::new(Mutex::new(MGMData::new())), } } - pub fn periodic_op(&mut self){ + pub fn get_data_ref(&mut self) -> Arc> { + self.mgm_data.clone() + } + + pub fn periodic_op(&mut self) { self.update_mode(); //self.handle_action_requests(); } @@ -124,13 +162,13 @@ impl MGMHandler { pub fn handle_hk_request(&mut self, hk_req: HkRequest) { match hk_req { - HkRequest::OneShot(_) => {} + HkRequest::OneShot(_) => { + + } HkRequest::Enable(_) => {} HkRequest::Disable(_) => {} HkRequest::ModifyCollectionInterval(_, _) => {} } } - pub fn handle_action_request(&mut self, action_request: ActionRequest) { - - } + pub fn handle_action_request(&mut self, action_request: ActionRequest) {} } diff --git a/src/can.rs b/src/can.rs index 679ad08..37647df 100644 --- a/src/can.rs +++ b/src/can.rs @@ -1,7 +1,3 @@ -#![allow(dead_code)] -#![allow(unused_variables)] -#![allow(unused_imports)] - use crate::power_handler::DeviceState; use crate::can_ids::{ diff --git a/src/hk.rs b/src/hk.rs index 4442f8d..010fd5a 100644 --- a/src/hk.rs +++ b/src/hk.rs @@ -1,22 +1,24 @@ -use std::ops::{Deref, DerefMut}; -use std::sync::{Arc, Mutex}; -use std::sync::mpsc::{Receiver, Sender}; +use crate::aocs_handler::{AocsSensorHandler, MGMData, MGMHandler}; +use crate::requests::Request; +use crate::requests::RequestWithToken; +use crate::tmtc::TmStore; +use eurosim_obsw::{hk_err, tmtc_err}; use satrs_core::pool::StoreAddr; use satrs_core::pus::hk::Subservice; -use satrs_core::pus::verification::{FailParams, StdVerifSenderError, VerificationReporterWithSender}; +use satrs_core::pus::verification::{ + FailParams, StdVerifSenderError, VerificationReporterWithSender, +}; use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore}; -use satrs_core::spacepackets::SpHeader; use satrs_core::spacepackets::time::cds::TimeProvider; use satrs_core::spacepackets::time::TimeWriter; use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader}; +use satrs_core::spacepackets::SpHeader; use satrs_core::tmtc::AddressableId; -use crate::aocs_handler::{AocsSensorHandler, MGMHandler}; -use crate::requests::RequestWithToken; -use crate::tmtc::TmStore; +use serde::{Deserialize, Serialize}; use serde_json::json; -use serde::{Serialize, Deserialize}; -use eurosim_obsw::{hk_err, tmtc_err}; -use crate::requests::Request; +use std::ops::{Deref, DerefMut}; +use std::sync::mpsc::{Receiver, Sender}; +use std::sync::{Arc, Mutex}; pub type CollectionIntervalFactor = u32; @@ -36,21 +38,26 @@ pub enum HkRequest { #[derive(Serialize, Deserialize)] pub struct AocsSensorData { - mgm_data: [f64; 3], // Voltage for 3 axis - css_data: [f64; 18], // Voltage for 18 sun sensors - str_data: [f64; 4], // Quaternion for position of satellite + mgm_data: MGMData, // Voltage for 3 axis + css_data: [f64; 18], // Voltage for 18 sun sensors + str_data: [f64; 4], // Quaternion for position of satellite } impl AocsSensorData { pub fn new() -> AocsSensorData { - let mgm_data = [0.0; 3]; + let mgm_data = MGMData::default(); let css_data = [0.0; 18]; let str_data = [0.0; 4]; - AocsSensorData{mgm_data, css_data, str_data} + AocsSensorData { + mgm_data, + css_data, + str_data, + } } - pub fn write_mgm_data(&mut self, mgm_data: [f64; 3]) { - self.mgm_data = mgm_data; + pub fn update_mgm_data(&mut self, mgm_data: &Arc>) { + let data = mgm_data.lock().unwrap(); + self.mgm_data = *data; } pub fn write_css_data(&mut self, css_data: [f64; 18]) { @@ -61,7 +68,7 @@ impl AocsSensorData { self.str_data = str_data; } - pub fn read_mgm_data(&mut self) -> [f64; 3] { + pub fn get_mgm_data(&mut self) -> MGMData { self.mgm_data } @@ -74,24 +81,25 @@ impl AocsSensorData { } } -pub struct AocsHkHandler { +pub struct AocsHousekeeper { sensor_data_pool: Arc>, action_rx: Receiver, seq_count_provider: SeqCountProviderSyncClonable, aocs_tm_store: TmStore, aocs_tm_funnel_tx: Sender, - verif_reporter: &'static mut VerificationReporterWithSender + verif_reporter: VerificationReporterWithSender, } -impl AocsHkHandler { - pub fn new(sensor_data_pool: Arc>, - action_rx: Receiver, - seq_count_provider: SeqCountProviderSyncClonable, - aocs_tm_store: TmStore, - aocs_tm_funnel_tx: Sender, - verif_reporter: &'static mut VerificationReporterWithSender, - ) -> AocsHkHandler { - AocsHkHandler { +impl AocsHousekeeper { + pub fn new( + sensor_data_pool: Arc>, + action_rx: Receiver, + seq_count_provider: SeqCountProviderSyncClonable, + aocs_tm_store: TmStore, + aocs_tm_funnel_tx: Sender, + verif_reporter: VerificationReporterWithSender, + ) -> AocsHousekeeper { + AocsHousekeeper { sensor_data_pool, action_rx, seq_count_provider, @@ -106,56 +114,52 @@ impl AocsHkHandler { if let Ok(request_with_token) = self.action_rx.try_recv() { if let Request::HkRequest(hk_req) = request_with_token.0 { - let cds_stamp = - TimeProvider::from_now_with_u16_days().unwrap(); + let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap(); cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap(); - let start_token = self //implement this for verification + let start_token = self //implement this for verification .verif_reporter .start_success(request_with_token.1, Some(&time_stamp_buf)) .expect("Error sending start success"); if let Ok(()) = match hk_req { - HkRequest::OneShot(id) => { - self.one_shot_hk(id) - } - HkRequest::Enable(id) => { - self.enable_hk(id) - } - HkRequest::Disable(id) => { - self.disable_hk(id) - } - HkRequest::ModifyCollectionInterval(id, collection_interval) => { - Ok(()) - } + HkRequest::OneShot(id) => self.one_shot_hk(id), + HkRequest::Enable(id) => self.enable_hk(id), + HkRequest::Disable(id) => self.disable_hk(id), + HkRequest::ModifyCollectionInterval(id, collection_interval) => Ok(()), } { - let cds_stamp = - TimeProvider::from_now_with_u16_days().unwrap(); + let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap(); cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap(); self.verif_reporter .completion_success(start_token, Some(&time_stamp_buf)) .expect("Error sending completion success"); } else { - let cds_stamp = - TimeProvider::from_now_with_u16_days().unwrap(); + let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap(); cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap(); self.verif_reporter - .completion_failure(start_token, FailParams::new(Some(&time_stamp_buf), &hk_err::UNKNOWN_TARGET_ID, None)) + .completion_failure( + start_token, + FailParams::new( + Some(&time_stamp_buf), + &hk_err::UNKNOWN_TARGET_ID, + None, + ), + ) .expect("Error sending completion success"); } } } } - pub fn one_shot_hk(&mut self, id: AddressableId) -> Result<(), ()>{ + pub fn one_shot_hk(&mut self, id: AddressableId) -> Result<(), ()> { let json_string = self.aocs_data_to_str(); self.send_hk_packet(id, &json_string); Ok(()) } - pub fn enable_hk(&mut self, id: AddressableId)-> Result<(), ()> { + pub fn enable_hk(&mut self, id: AddressableId) -> Result<(), ()> { Ok(()) } - pub fn disable_hk(&mut self, id: AddressableId) -> Result<(), ()>{ + pub fn disable_hk(&mut self, id: AddressableId) -> Result<(), ()> { Ok(()) } @@ -168,29 +172,16 @@ impl AocsHkHandler { let mut time_stamp_buf: [u8; 7] = [0; 7]; let mut huge_buf: [u8; 8192] = [0; 8192]; - let mut sp_header = SpHeader::tm_unseg( - 0x02, - self.seq_count_provider.get_and_increment(), - 0, - ).unwrap(); - let cds_stamp = - TimeProvider::from_now_with_u16_days().unwrap(); + let mut sp_header = + SpHeader::tm_unseg(0x02, self.seq_count_provider.get_and_increment(), 0).unwrap(); + let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap(); cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap(); let mut len = id.write_to_be_bytes(&mut huge_buf).unwrap(); - huge_buf[8..data.len() + 8] - .copy_from_slice(data.as_bytes()); + huge_buf[8..data.len() + 8].copy_from_slice(data.as_bytes()); len += data.len(); - let tm_sec_header = PusTmSecondaryHeader::new_simple( - 3, - Subservice::TmHkPacket as u8, - &time_stamp_buf, - ); - let hk_tm = PusTm::new( - &mut sp_header, - tm_sec_header, - Some(&huge_buf[0..len]), - true, - ); + let tm_sec_header = + PusTmSecondaryHeader::new_simple(3, Subservice::TmHkPacket as u8, &time_stamp_buf); + let hk_tm = PusTm::new(&mut sp_header, tm_sec_header, Some(&huge_buf[0..len]), true); let addr = self.aocs_tm_store.add_pus_tm(&hk_tm); self.aocs_tm_funnel_tx.send(addr).expect("sending failed"); } diff --git a/src/main.rs b/src/main.rs index 1f5aa78..da730a9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -39,6 +39,7 @@ use strum::IntoEnumIterator; use crate::can_ids::{ can_id_to_package_id, load_package_ids, DeviceId, PackageId, PackageModel, ThreadId, }; +#[cfg(feature = "can")] use embedded_can::{Id, StandardId}; use log::{info, warn}; use satrs_core::power::{SwitchId, SwitchState}; @@ -49,7 +50,9 @@ use std::sync::{mpsc, Arc, Mutex, RwLock}; use std::thread; //use libc::time64_t; use crate::action::ActionRequest; +use crate::aocs_handler::MGMData; use crate::cam::CameraRequest; +use crate::hk::{AocsHousekeeper, AocsSensorData}; use crate::pld_handler::{core_pld_task, CameraHandler}; use crate::power_handler::{core_power_task, PowerSwitcher}; @@ -144,9 +147,9 @@ fn main() { event_man.subscribe_all(pus_event_man_send_provider.id()); let mut request_map = HashMap::new(); - let (acs_thread_tx, acs_thread_rx) = channel::(); + let (aocs_thread_tx, aocs_thread_rx) = channel::(); let (pld_thread_tx, pld_thread_rx) = channel::(); - request_map.insert(RequestTargetId::AcsSubsystem as u32, acs_thread_tx); + request_map.insert(RequestTargetId::AcsSubsystem as u32, aocs_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); @@ -187,14 +190,14 @@ fn main() { // get package id hashmap let package_ids_rx = load_package_ids(); - let socket0 = can::CanRxHandler::new_socket("can0", can_senders, package_ids_rx).unwrap(); - info!("Starting TMTC task"); let builder0 = thread::Builder::new().name("TMTCThread".into()); let jh0 = builder0.spawn(move || { core_tmtc_task(core_args, tc_args, tm_args); }); + let socket0 = can::CanRxHandler::new_socket("can0", can_senders, package_ids_rx).unwrap(); + info!("Starting CAN Socket listening task"); let builder1 = thread::Builder::new().name("CanRxHandler".into()); let jh1 = builder1.spawn(move || loop { @@ -227,10 +230,6 @@ fn main() { ); }); - let package_map_aocs_tx = load_package_ids(); - let aocs_tm_funnel_tx = tm_funnel_tx.clone(); - let mut aocs_tm_store = tm_store.clone(); - /* // AOCS Thread let socket1 = @@ -359,6 +358,30 @@ fn main() { } }); + let package_map_aocs_tx = load_package_ids(); + let aocs_tm_funnel_tx = tm_funnel_tx.clone(); + let mut aocs_tm_store = tm_store.clone(); + + let mut mgm_shared_data: Arc> = Arc::default(); + + let aocs_sensor_data = Arc::new(Mutex::new(AocsSensorData::new())); + + info!("Starting AOCS task"); + let builder5 = thread::Builder::new().name("AOCSThread".into()); + let jh5 = builder5.spawn(move || { + let mut aocs_housekeeper = AocsHousekeeper::new( + aocs_sensor_data.clone(), + aocs_thread_rx, + aocs_seq_count_provider, + aocs_tm_store, + aocs_tm_funnel_tx, + reporter_aocs, + ); + loop { + aocs_housekeeper.handle_hk_request(); + } + }); + jh0.unwrap() .join() .expect("Joining UDP TMTC server thread failed"); @@ -370,6 +393,7 @@ fn main() { jh4.unwrap() .join() .expect("Joining TM funnel thread failed"); + jh5.unwrap().join().expect("Joining AOCS thread failed"); } #[derive(Default)] struct MgmData {