ability to send first housekeeping packets

src/aocs_handler.rs

@@ -1,11 +1,13 @@
-use std::sync::mpsc::{Receiver, TryRecvError};
+use std::sync::{Arc, Mutex};
-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;
+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<AocsSensorMode>,
     action_rx: Receiver<RequestWithToken>,
+    mgm_data: Arc<Mutex<MGMData>>,
 }
 
 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<Mutex<MGMData>> {
+        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) {}
-
-    }
 }

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::{

src/hk.rs

@@ -1,22 +1,24 @@
-use std::ops::{Deref, DerefMut};
+use crate::aocs_handler::{AocsSensorHandler, MGMData, MGMHandler};
-use std::sync::{Arc, Mutex};
+use crate::requests::Request;
-use std::sync::mpsc::{Receiver, Sender};
+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 serde::{Deserialize, Serialize};
-use crate::requests::RequestWithToken;
-use crate::tmtc::TmStore;
 use serde_json::json;
-use serde::{Serialize, Deserialize};
+use std::ops::{Deref, DerefMut};
-use eurosim_obsw::{hk_err, tmtc_err};
+use std::sync::mpsc::{Receiver, Sender};
-use crate::requests::Request;
+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
+    mgm_data: MGMData,  // Voltage for 3 axis
-    css_data: [f64; 18],    // Voltage for 18 sun sensors
+    css_data: [f64; 18], // Voltage for 18 sun sensors
-    str_data: [f64; 4],     // Quaternion for position of satellite
+    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]) {
+    pub fn update_mgm_data(&mut self, mgm_data: &Arc<Mutex<MGMData>>) {
-        self.mgm_data = mgm_data;
+        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<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>
+    verif_reporter: VerificationReporterWithSender<StdVerifSenderError>,
 }
 
-impl AocsHkHandler {
+impl AocsHousekeeper {
-    pub fn new(sensor_data_pool: Arc<Mutex<AocsSensorData>>,
+    pub fn new(
-               action_rx: Receiver<RequestWithToken>,
+        sensor_data_pool: Arc<Mutex<AocsSensorData>>,
-               seq_count_provider: SeqCountProviderSyncClonable,
+        action_rx: Receiver<RequestWithToken>,
-               aocs_tm_store: TmStore,
+        seq_count_provider: SeqCountProviderSyncClonable,
-               aocs_tm_funnel_tx: Sender<StoreAddr>,
+        aocs_tm_store: TmStore,
-               verif_reporter: &'static mut VerificationReporterWithSender<StdVerifSenderError>,
+        aocs_tm_funnel_tx: Sender<StoreAddr>,
-    ) -> AocsHkHandler {
+        verif_reporter: VerificationReporterWithSender<StdVerifSenderError>,
-        AocsHkHandler {
+    ) -> 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 =
+                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
-                    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) => {
+                    HkRequest::OneShot(id) => self.one_shot_hk(id),
-                        self.one_shot_hk(id)
+                    HkRequest::Enable(id) => self.enable_hk(id),
-                    }
+                    HkRequest::Disable(id) => self.disable_hk(id),
-                    HkRequest::Enable(id) => {
+                    HkRequest::ModifyCollectionInterval(id, collection_interval) => Ok(()),
-                        self.enable_hk(id)
-                    }
-                    HkRequest::Disable(id) => {
-                        self.disable_hk(id)
-                    }
-                    HkRequest::ModifyCollectionInterval(id, collection_interval) => {
-                        Ok(())
-                    }
                 } {
-                    let cds_stamp =
+                    let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
-                        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 =
+                    let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
-                        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(
+        let mut sp_header =
-            0x02,
+            SpHeader::tm_unseg(0x02, self.seq_count_provider.get_and_increment(), 0).unwrap();
-            self.seq_count_provider.get_and_increment(),
+        let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
-            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]
+        huge_buf[8..data.len() + 8].copy_from_slice(data.as_bytes());
-            .copy_from_slice(data.as_bytes());
         len += data.len();
-        let tm_sec_header = PusTmSecondaryHeader::new_simple(
+        let tm_sec_header =
-            3,
+            PusTmSecondaryHeader::new_simple(3, Subservice::TmHkPacket as u8, &time_stamp_buf);
-            Subservice::TmHkPacket as u8,
+        let hk_tm = PusTm::new(&mut sp_header, tm_sec_header, Some(&huge_buf[0..len]), true);
-            &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/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::<RequestWithToken>();
+    let (aocs_thread_tx, aocs_thread_rx) = channel::<RequestWithToken>();
     let (pld_thread_tx, pld_thread_rx) = channel::<RequestWithToken>();
-    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<Mutex<MGMData>> = 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 {