added aocs housekeeper

Cargo.lock

@@ -812,9 +812,9 @@ dependencies = [
 
 [[package]]
 name = "spacepackets"
-version = "0.5.3"
+version = "0.5.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "81541fd89a5bc02845a849895d6ed1721235b3eac26fc77010f0a05f53bc4e8a"
+checksum = "9313bf066de3ae704ea83584fc65b62434249efe57ae2dcde3e2cffe4ba62648"
 dependencies = [
  "chrono",
  "crc",

src/aocs_handler.rs

@@ -1 +1,136 @@
+use std::sync::mpsc::{Receiver, TryRecvError};
+use satrs_core::power::SwitchId;
+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;
 
+pub enum AocsSensorMode {
+    Idle,
+    SendingData,
+}
+
+pub trait AocsSensorHandler {
+    type Error;
+
+    fn get_package_id(&mut self) -> Result<PackageId, Self::Error>;
+    fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error>;
+
+    fn enable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
+        let id = self.get_package_id()?;
+        self.send_message(id, &[1])
+    }
+
+    fn disable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
+        let id = self.get_package_id()?;
+        self.send_message(id, &[0])
+    }
+
+    fn request_sensor_data_oneshot(&mut self) -> Result<(), Self::Error> {
+        let id = self.get_package_id()?;
+        self.send_message(id, &[2])
+    }
+
+    fn write_to_shared_pool(&mut self) -> Result<(), Self::Error>;
+}
+
+pub struct MGMHandler {
+    power_switcher: PowerSwitcher,
+    device_id: DeviceId,
+    switch_id: SwitchId,
+    device_state: DeviceState,
+    can_tx: CanTxHandler,
+    can_rx: Receiver<PackageModel>,
+    mode: AocsSensorMode,
+    mode_rx: Receiver<AocsSensorMode>,
+    action_rx: Receiver<RequestWithToken>,
+}
+
+impl AocsSensorHandler for MGMHandler {
+    type Error = ();
+
+    fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
+        return match self.device_id {
+            DeviceId::MGM1 => Ok(PackageId::AOCSDataRequestMGM1),
+            DeviceId::MGM2 => Ok(PackageId::AOCSDataRequestMGM2),
+            DeviceId::MGM3 => Ok(PackageId::AOCSDataRequestMGM3),
+            DeviceId::MGM4 => Ok(PackageId::AOCSDataRequestMGM4),
+            _ => 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 {
+    pub fn new(
+        power_switcher: PowerSwitcher,
+        device_id: DeviceId,
+        switch_id: SwitchId,
+        can_tx: CanTxHandler,
+        can_rx: Receiver<PackageModel>,
+        mode_rx: Receiver<AocsSensorMode>,
+        action_rx: Receiver<RequestWithToken>,
+    ) -> MGMHandler {
+        MGMHandler {
+            power_switcher,
+            device_id,
+            switch_id,
+            device_state: DeviceState::Off,
+            can_tx,
+            can_rx,
+            mode: AocsSensorMode::Idle,
+            mode_rx,
+            action_rx,
+        }
+    }
+
+    pub fn periodic_op(&mut self){
+        self.update_mode();
+        //self.handle_action_requests();
+    }
+
+    pub fn update_mode(&mut self) {
+        if self.device_state == DeviceState::On {
+            if let Ok(mode) = self.mode_rx.try_recv() {
+                self.mode = mode;
+            }
+        }
+    }
+
+    pub fn handle_requests(&mut self) {
+        if self.device_state == DeviceState::On {
+            if let Ok(request) = self.action_rx.try_recv() {
+                match request.0 {
+                    Request::HkRequest(hk_req) => {
+                        self.handle_hk_request(hk_req);
+                    }
+                    Request::ActionRequest(action_request) => {
+                        self.handle_action_request(action_request);
+                    }
+                }
+            }
+        }
+    }
+
+    pub fn handle_hk_request(&mut self, hk_req: HkRequest) {
+        match hk_req {
+            HkRequest::OneShot(_) => {}
+            HkRequest::Enable(_) => {}
+            HkRequest::Disable(_) => {}
+            HkRequest::ModifyCollectionInterval(_, _) => {}
+        }
+    }
+    pub fn handle_action_request(&mut self, action_request: ActionRequest) {
+
+    }
+}

src/hk.rs

@@ -1,10 +1,29 @@
+use std::ops::{Deref, DerefMut};
+use std::sync::{Arc, Mutex};
+use std::sync::mpsc::{Receiver, Sender};
+use satrs_core::pool::StoreAddr;
+use satrs_core::pus::hk::Subservice;
+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::tmtc::AddressableId;
+use crate::aocs_handler::{AocsSensorHandler, MGMHandler};
+use crate::requests::RequestWithToken;
+use crate::tmtc::TmStore;
+use serde_json::json;
+use serde::{Serialize, Deserialize};
+use eurosim_obsw::{hk_err, tmtc_err};
+use crate::requests::Request;
 
 pub type CollectionIntervalFactor = u32;
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 pub enum AocsHkIds {
-    TestMgmSet = 1,
+    TestAocsSet = 1,
+    TestMgmSet = 2,
 }
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
@@ -15,4 +34,164 @@ pub enum HkRequest {
     ModifyCollectionInterval(AddressableId, CollectionIntervalFactor),
 }
 
-pub struct AocsHkHandler {}
+#[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
+}
+
+impl AocsSensorData {
+    pub fn new() -> AocsSensorData {
+        let mgm_data = [0.0; 3];
+        let css_data = [0.0; 18];
+        let str_data = [0.0; 4];
+        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 write_css_data(&mut self, css_data: [f64; 18]) {
+        self.css_data = css_data;
+    }
+
+    pub fn write_str_data(&mut self, str_data: [f64; 4]) {
+        self.str_data = str_data;
+    }
+
+    pub fn read_mgm_data(&mut self) -> [f64; 3] {
+        self.mgm_data
+    }
+
+    pub fn read_css_data(&mut self) -> [f64; 18] {
+        self.css_data
+    }
+
+    pub fn read_str_data(&mut self) -> [f64; 4] {
+        self.str_data
+    }
+}
+
+pub struct AocsHkHandler {
+    sensor_data_pool: Arc<Mutex<AocsSensorData>>,
+    action_rx: Receiver<RequestWithToken>,
+    seq_count_provider: SeqCountProviderSyncClonable,
+    aocs_tm_store: TmStore,
+    aocs_tm_funnel_tx: Sender<StoreAddr>,
+    verif_reporter: &'static mut VerificationReporterWithSender<StdVerifSenderError>
+}
+
+impl AocsHkHandler {
+    pub fn new(sensor_data_pool: Arc<Mutex<AocsSensorData>>,
+               action_rx: Receiver<RequestWithToken>,
+               seq_count_provider: SeqCountProviderSyncClonable,
+               aocs_tm_store: TmStore,
+               aocs_tm_funnel_tx: Sender<StoreAddr>,
+               verif_reporter: &'static mut VerificationReporterWithSender<StdVerifSenderError>,
+    ) -> AocsHkHandler {
+        AocsHkHandler {
+            sensor_data_pool,
+            action_rx,
+            seq_count_provider,
+            aocs_tm_store,
+            aocs_tm_funnel_tx,
+            verif_reporter,
+        }
+    }
+
+    pub fn handle_hk_request(&mut self) {
+        let mut time_stamp_buf: [u8; 7] = [0; 7];
+
+        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();
+                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+                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(())
+                    }
+                } {
+                    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();
+                    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))
+                        .expect("Error sending completion success");
+                }
+            }
+        }
+    }
+
+    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<(), ()> {
+        Ok(())
+    }
+
+    pub fn disable_hk(&mut self, id: AddressableId) -> Result<(), ()>{
+        Ok(())
+    }
+
+    pub fn aocs_data_to_str(&mut self) -> String {
+        let pool = self.sensor_data_pool.lock().unwrap();
+        serde_json::to_string(pool.deref()).unwrap()
+    }
+
+    pub fn send_hk_packet(&mut self, id: AddressableId, data: &str) {
+        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();
+        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());
+        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 addr = self.aocs_tm_store.add_pus_tm(&hk_tm);
+        self.aocs_tm_funnel_tx.send(addr).expect("sending failed");
+    }
+}

src/tmtc.rs

@@ -16,7 +16,7 @@ use crate::pus::PusReceiver;
 use crate::requests::RequestWithToken;
 use satrs_core::pool::{SharedPool, StoreAddr, StoreError};
 use satrs_core::pus::event_man::EventRequestWithToken;
-use satrs_core::pus::scheduling::PusScheduler;
+use satrs_core::pus::scheduling::{PusScheduler, TcInfo};
 use satrs_core::pus::verification::StdVerifReporterWithSender;
 use satrs_core::spacepackets::{ecss::PusPacket, tc::PusTc, tm::PusTm, SpHeader};
 use satrs_core::tmtc::{
@@ -219,11 +219,15 @@ fn core_tmtc_loop(
     pus_receiver: &mut PusReceiver,
     scheduler: Rc<RefCell<PusScheduler>>,
 ) {
-    let releaser = |enabled: bool, addr: &StoreAddr| -> bool {
-        match tc_args.tc_source.tc_source.send(*addr) {
-            Ok(_) => true,
-            Err(_) => false,
+    let releaser = |enabled: bool, info: &TcInfo| -> bool {
+        if enabled {
+            tc_args
+                .tc_source
+                .tc_source
+                .send(info.addr())
+                .expect("sending TC to TC source failed");
         }
+        true
     };
 
     let mut pool = tc_args
@@ -235,15 +239,11 @@ fn core_tmtc_loop(
 
     let mut scheduler = scheduler.borrow_mut();
     scheduler.update_time_from_now().unwrap();
-    match scheduler.release_telecommands(releaser, pool.as_mut()) {
-        Ok(released_tcs) => {
-            if released_tcs > 0 {
-                println!("{} Tc(s) released from scheduler", released_tcs);
-            }
+    if let Ok(released_tcs) = scheduler.release_telecommands(releaser, pool.as_mut()) {
+        if released_tcs > 0 {
+            println!("{released_tcs} TC(s) released from scheduler");
         }
-        Err(_) => {}
     }
-    //.expect("error releasing tc");
     drop(pool);
     drop(scheduler);