updates

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<AocsSensorMode>,
     action_rx: Receiver<RequestWithToken>,
     mgm_data: Arc<Mutex<MGMData>>,
+    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<PackageModel>,
         can_rx: Receiver<PackageModel>,
         mode_rx: Receiver<AocsSensorMode>,
         action_rx: Receiver<RequestWithToken>,
     ) -> 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) {}
 }

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<Mutex<MGMData>> = 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();
         }
     });