added gps handler

src/aocs.rs

@@ -3,7 +3,7 @@ use crate::aocs::AOCSSensorMode::{Idle, SendingData};
 use crate::can_ids::PackageId::AOCSDataStarTracker1;
 use crate::can_ids::{DeviceId, PackageId, PackageModel};
 use crate::helpers::{ModeHelper, VerifHelper};
-use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper, CSS_VOLTAGE_1, CSS_VOLTAGE_2, CSS_VOLTAGE_3, CSS_VOLTAGE_4, CSS_VOLTAGE_5, CSS_VOLTAGE_6, MGM_VOLTAGE_1, MGM_VOLTAGE_2, MGM_VOLTAGE_3, STR_QUATERNION_1, STR_QUATERNION_2, STR_QUATERNION_3, STR_QUATERNION_4};
+use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper, CSS_SUN_VECTOR_1, CSS_SUN_VECTOR_2, CSS_SUN_VECTOR_3, CSS_VOLTAGE_4, CSS_VOLTAGE_5, CSS_VOLTAGE_6, GPS_ALTITUDE, GPS_LATITUDE, GPS_LONGITUDE, MGM_FIELD_1, MGM_FIELD_2, MGM_FIELD_3, STR_QUATERNION_1, STR_QUATERNION_2, STR_QUATERNION_3, STR_QUATERNION_4};
 use crate::power_handler::{DeviceState, PowerSwitcher};
 use crate::requests::{Request, RequestWithToken};
 use crate::tmtc::TmStore;
@@ -23,6 +23,7 @@ use serde::{Deserialize, Serialize};
 use std::sync::mpsc::{channel, Receiver, Sender, TryRecvError};
 use std::sync::{Arc, Mutex};
 use std::u32;
+use log::debug;
 
 #[derive(ToPrimitive, PartialEq, Copy, Clone)]
 pub enum AOCSSensorMode {
@@ -131,7 +132,7 @@ impl CSSHandler {
                 Request::ModeRequest(req) => match req {
                     ModeRequest::SetMode(mode_submode) => {
                         if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
-                            self.mode = aocs_mode;
+                            self.update_mode(aocs_mode);
                         }
 
                         if let Some(token) = start_token {
@@ -179,6 +180,16 @@ impl CSSHandler {
             }
         }
     }
+
+    fn update_mode(&mut self, mode: AOCSSensorMode) {
+        if mode != self.mode {
+            match mode {
+                Idle => { self.set_mode_idle() }
+                SendingData => { self.set_mode_reading() }
+            }
+        }
+    }
+
     fn set_mode_reading(&mut self) {
         if self.mode == SendingData {
             return;
@@ -212,15 +223,15 @@ impl CSSHandler {
             let float_data = self.decode_sensor_data(package.data());
             match package.package_id() {
                 PackageId::AOCSDataSunSensor1 => {
-                    map.update_value(CSS_VOLTAGE_1, AocsDataType::float_value(float_data))
+                    map.update_value(CSS_SUN_VECTOR_1, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
                 PackageId::AOCSDataSunSensor2 => {
-                    map.update_value(CSS_VOLTAGE_2, AocsDataType::float_value(float_data))
+                    map.update_value(CSS_SUN_VECTOR_2, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
                 PackageId::AOCSDataSunSensor3 => {
-                    map.update_value(CSS_VOLTAGE_3, AocsDataType::float_value(float_data))
+                    map.update_value(CSS_SUN_VECTOR_3, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
                 PackageId::AOCSDataSunSensor4 => {
@@ -310,6 +321,7 @@ impl MGMHandler {
 
     fn handle_requests(&mut self) {
         if let Ok(req) = self.request_rx.try_recv() {
+            debug!("request received");
             let (req, start_token) = self
                 .verif_helper
                 .start_and_unwrap(req)
@@ -325,7 +337,7 @@ impl MGMHandler {
                 Request::ModeRequest(req) => match req {
                     ModeRequest::SetMode(mode_submode) => {
                         if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
-                            self.mode = aocs_mode;
+                            self.update_mode(aocs_mode);
                         }
 
                         if let Some(token) = start_token {
@@ -373,6 +385,16 @@ impl MGMHandler {
             }
         }
     }
+
+    fn update_mode(&mut self, mode: AOCSSensorMode) {
+        if mode != self.mode {
+            match mode {
+                Idle => { self.set_mode_idle() }
+                SendingData => { self.set_mode_reading() }
+            }
+        }
+    }
+
     fn set_mode_reading(&mut self) {
         if self.mode == SendingData {
             return;
@@ -405,20 +427,16 @@ impl MGMHandler {
             let mut map = self.aocs_data.lock().expect("error locking data map");
             let float_data = self.decode_sensor_data(package.data());
             match package.package_id() {
-                PackageId::AOCSDataStarTracker1 => {
+                PackageId::AOCSDataMGM1 => {
-                    map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data))
+                    map.update_value(MGM_FIELD_1, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
-                PackageId::AOCSDataStarTracker2 => {
+                PackageId::AOCSDataMGM2 => {
-                    map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data))
+                    map.update_value(MGM_FIELD_2, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
-                PackageId::AOCSDataStarTracker3 => {
+                PackageId::AOCSDataMGM3 => {
-                    map.update_value(STR_QUATERNION_3, AocsDataType::float_value(float_data))
+                    map.update_value(MGM_FIELD_3, AocsDataType::float_value(float_data))
-                        .expect("error updating value");
-                }
-                PackageId::AOCSDataStarTracker4 => {
-                    map.update_value(STR_QUATERNION_4, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
                 _ => {}
@@ -511,7 +529,7 @@ impl STRHandler {
                 Request::ModeRequest(req) => match req {
                     ModeRequest::SetMode(mode_submode) => {
                         if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
-                            self.mode = aocs_mode;
+                            self.update_mode(aocs_mode);
                         }
 
                         if let Some(token) = start_token {
@@ -560,6 +578,15 @@ impl STRHandler {
         }
     }
 
+    fn update_mode(&mut self, mode: AOCSSensorMode) {
+        if mode != self.mode {
+            match mode {
+                Idle => { self.set_mode_idle() }
+                SendingData => { self.set_mode_reading() }
+            }
+        }
+    }
+
     fn set_mode_reading(&mut self) {
         if self.mode == SendingData {
             return;
@@ -592,16 +619,212 @@ impl STRHandler {
             let mut map = self.aocs_data.lock().expect("error locking data map");
             let float_data = self.decode_sensor_data(package.data());
             match package.package_id() {
-                PackageId::AOCSDataMGM1 => {
+                PackageId::AOCSDataStarTracker1 => {
-                    map.update_value(MGM_VOLTAGE_1, AocsDataType::float_value(float_data))
+                    map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
-                PackageId::AOCSDataMGM2 => {
+                PackageId::AOCSDataStarTracker2 => {
-                    map.update_value(MGM_VOLTAGE_2, AocsDataType::float_value(float_data))
+                    map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
-                PackageId::AOCSDataMGM2 => {
+                PackageId::AOCSDataStarTracker3 => {
-                    map.update_value(MGM_VOLTAGE_3, AocsDataType::float_value(float_data))
+                    map.update_value(STR_QUATERNION_3, AocsDataType::float_value(float_data))
+                        .expect("error updating value");
+                }
+                PackageId::AOCSDataStarTracker4 => {
+                    map.update_value(STR_QUATERNION_4, AocsDataType::float_value(float_data))
+                        .expect("error updating value");
+                }
+                _ => {}
+            }
+            drop(map);
+        }
+    }
+
+    fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
+        LittleEndian::read_f64(&buf)
+    }
+}
+
+pub struct GPSHandler {
+    power_switcher: PowerSwitcher,
+    device_id: DeviceId,
+    switch_id: SwitchId,
+    device_state: DeviceState,
+    mode: AOCSSensorMode,
+    aocs_data: Arc<Mutex<AocsDataMap>>,
+    can_tx: Sender<PackageModel>,
+    can_rx: Receiver<PackageModel>,
+    request_rx: Receiver<RequestWithToken>,
+    verif_helper: VerifHelper,
+    mode_helper: ModeHelper,
+}
+
+impl AocsSensorHandler for GPSHandler {
+    type Error = ();
+
+    fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
+        Ok(PackageId::AOCSDataRequestGPS)
+    }
+
+    fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error> {
+        self.can_tx
+            .send(PackageModel::new(id, buf).unwrap())
+            .unwrap();
+        return Ok(());
+    }
+}
+
+impl GPSHandler {
+    pub fn new(
+        power_switcher: PowerSwitcher,
+        device_id: DeviceId,
+        device_state: DeviceState,
+        aocs_data: Arc<Mutex<AocsDataMap>>,
+        can_tx: Sender<PackageModel>,
+        can_rx: Receiver<PackageModel>,
+        request_rx: Receiver<RequestWithToken>,
+        verif_helper: VerifHelper,
+        mode_helper: ModeHelper,
+    ) -> Self {
+        let switch_id = device_id as u16;
+        Self {
+            power_switcher,
+            device_id,
+            switch_id,
+            device_state,
+            mode: Idle,
+            aocs_data,
+            can_tx,
+            can_rx,
+            request_rx,
+            verif_helper,
+            mode_helper,
+        }
+    }
+
+    pub fn periodic_op(&mut self) {
+        self.handle_requests();
+        self.read_can();
+    }
+
+    fn handle_requests(&mut self) {
+        if let Ok(req) = self.request_rx.try_recv() {
+            let (req, start_token) = self
+                .verif_helper
+                .start_and_unwrap(req)
+                .expect("error sending start of execution");
+            match req {
+                Request::HkRequest(_) => {
+                    if let Some(token) = start_token {
+                        self.verif_helper
+                            .completion_failure(token)
+                            .expect("error sending completion failure");
+                    }
+                }
+                Request::ModeRequest(req) => match req {
+                    ModeRequest::SetMode(mode_submode) => {
+                        if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
+                            self.update_mode(aocs_mode);
+                        }
+
+                        if let Some(token) = start_token {
+                            self.verif_helper
+                                .completion(token)
+                                .expect("error sending completion success");
+                        }
+                    }
+                    ModeRequest::ReadMode => {
+                        self.mode_helper
+                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
+                            .expect("error sending mode reply");
+                        if let Some(token) = start_token {
+                            self.verif_helper
+                                .completion(token)
+                                .expect("error sending completion success");
+                        }
+                    }
+                    ModeRequest::AnnounceMode => {
+                        if let Some(token) = start_token {
+                            self.verif_helper
+                                .completion_failure(token)
+                                .expect("error sending completion failure");
+                        }
+                    }
+                    ModeRequest::AnnounceModeRecursive => {
+                        self.mode_helper
+                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
+                            .expect("error sending mode reply");
+
+                        if let Some(token) = start_token {
+                            self.verif_helper
+                                .completion(token)
+                                .expect("error sending completion failure");
+                        }
+                    }
+                },
+                Request::ActionRequest(_) => {
+                    if let Some(token) = start_token {
+                        self.verif_helper
+                            .completion_failure(token)
+                            .expect("error sending completion failure");
+                    }
+                }
+            }
+        }
+    }
+
+    fn update_mode(&mut self, mode: AOCSSensorMode) {
+        if mode != self.mode {
+            match mode {
+                Idle => { self.set_mode_idle() }
+                SendingData => { self.set_mode_reading() }
+            }
+        }
+    }
+
+    fn set_mode_reading(&mut self) {
+        if self.mode == SendingData {
+            return;
+        }
+
+        // TODO: currently unrecoverable, should probably be changed
+        self.power_switcher
+            .send_switch_on_cmd(self.switch_id)
+            .expect("error sending switch on cmd");
+        self.enable_sensor_data_generation()
+            .expect("error enabling sensor data generation");
+        self.mode = SendingData;
+    }
+
+    fn set_mode_idle(&mut self) {
+        if self.mode == Idle {
+            return;
+        }
+
+        self.disable_sensor_data_generation()
+            .expect("error disabling sensor data generation");
+        self.power_switcher
+            .send_switch_off_cmd(self.switch_id)
+            .expect("error sending switch off cmd");
+        self.mode = Idle;
+    }
+
+    fn read_can(&mut self) {
+        if let Ok(package) = self.can_rx.try_recv() {
+            let mut map = self.aocs_data.lock().expect("error locking data map");
+            let float_data = self.decode_sensor_data(package.data());
+            match package.package_id() {
+                PackageId::AOCSDataGPSLatitude => {
+                    map.update_value(GPS_LATITUDE, AocsDataType::float_value(float_data))
+                        .expect("error updating value");
+                }
+                PackageId::AOCSDataGPSLongitude => {
+                    map.update_value(GPS_LONGITUDE, AocsDataType::float_value(float_data))
+                        .expect("error updating value");
+                }
+                PackageId::AOCSDataGPSAltitude => {
+                    map.update_value(GPS_ALTITUDE, AocsDataType::float_value(float_data))
                         .expect("error updating value");
                 }
                 _ => {}
@@ -620,6 +843,7 @@ pub struct AocsController {
     mgm_request_tx: Sender<RequestWithToken>,
     css_request_tx: Sender<RequestWithToken>,
     str_request_tx: Sender<RequestWithToken>,
+    gps_request_tx: Sender<RequestWithToken>,
     seq_count_provider: SeqCountProviderSyncClonable,
     verif_helper: VerifHelper,
     mode_helper: ModeHelper,
@@ -632,6 +856,7 @@ impl AocsController {
         mgm_request_tx: Sender<RequestWithToken>,
         css_request_tx: Sender<RequestWithToken>,
         str_request_tx: Sender<RequestWithToken>,
+        gps_request_tx: Sender<RequestWithToken>,
         seq_count_provider: SeqCountProviderSyncClonable,
         verif_helper: VerifHelper,
         mode_helper: ModeHelper,
@@ -641,6 +866,7 @@ impl AocsController {
             mgm_request_tx,
             css_request_tx,
             str_request_tx,
+            gps_request_tx,
             seq_count_provider,
             verif_helper,
             mode_helper,
@@ -680,6 +906,9 @@ impl AocsController {
                         self.str_request_tx
                             .send(RequestWithToken(Request::ModeRequest(req), None))
                             .expect("error sending to str");
+                        self.gps_request_tx
+                            .send(RequestWithToken(Request::ModeRequest(req), None))
+                            .expect("error sending to str");
 
                         if let Some(token) = start_token {
                             self.verif_helper
@@ -718,6 +947,9 @@ impl AocsController {
                         self.str_request_tx
                             .send(RequestWithToken(Request::ModeRequest(req), None))
                             .expect("error sending mode request to str handler");
+                        self.gps_request_tx
+                            .send(RequestWithToken(Request::ModeRequest(req), None))
+                            .expect("error sending mode request to str handler");
 
                         if let Some(token) = start_token {
                             self.verif_helper

src/can_ids.rs

@@ -2,7 +2,7 @@ use embedded_can::{Id, StandardId};
 use log::warn;
 use std::collections::HashMap;
 
-use crate::tmtc::{CSS_APID, MGM_APID, MGT_APID, PLD_APID, PWR_APID, RWL_APID, STR_APID};
+use crate::tmtc::{CSS_APID, GPS_APID, MGM_APID, MGT_APID, PLD_APID, PWR_APID, RWL_APID, STR_APID};
 pub use num_derive::{FromPrimitive, ToPrimitive};
 pub use num_traits::{FromPrimitive, ToPrimitive};
 pub use strum::IntoEnumIterator; // 0.17.1
@@ -47,6 +47,7 @@ pub enum PackageId {
     AOCSDataRequestSunSensor5 = 49,
     AOCSDataRequestSunSensor6 = 50,
     AOCSDataRequestStarTracker = 51,
+    AOCSDataRequestGPS = 55,
     AOCSDataMGM1 = 61,
     AOCSDataMGM2 = 62,
     AOCSDataMGM3 = 63,
@@ -61,6 +62,9 @@ pub enum PackageId {
     AOCSDataStarTracker2 = 72,
     AOCSDataStarTracker3 = 73,
     AOCSDataStarTracker4 = 74,
+    AOCSDataGPSLatitude = 81,
+    AOCSDataGPSLongitude = 82,
+    AOCSDataGPSAltitude = 83,
     CameraImageRequest = 101,
     CameraImageRequestConfirmation = 102,
     CameraImageExecutionStart = 103,
@@ -92,6 +96,7 @@ pub enum DeviceId {
     RWL4 = 21,
     Camera = 22,
     All = 23,
+    GPS = 24,
 }
 
 impl TryFrom<u16> for DeviceId {
@@ -122,6 +127,7 @@ impl TryFrom<u16> for DeviceId {
             x if x == DeviceId::RWL4 as u16 => Ok(DeviceId::RWL4),
             x if x == DeviceId::Camera as u16 => Ok(DeviceId::Camera),
             x if x == DeviceId::All as u16 => Ok(DeviceId::All),
+            x if x == DeviceId::GPS as u16 => Ok(DeviceId::GPS),
             _ => Err(()),
         }
     }
@@ -216,6 +222,7 @@ pub fn load_package_id_to_threads() -> HashMap<PackageId, SenderReceiverThread>
         SenderReceiverThread::new(DeviceId::OBC, DeviceId::SunSensor5, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::OBC, DeviceId::SunSensor6, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::OBC, DeviceId::StarTracker, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::GPS, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::MGM1, DeviceId::OBC, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::MGM2, DeviceId::OBC, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::MGM3, DeviceId::OBC, ThreadId::AOCSThread),
@@ -230,6 +237,9 @@ pub fn load_package_id_to_threads() -> HashMap<PackageId, SenderReceiverThread>
         SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::GPS, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::GPS, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::GPS, DeviceId::OBC, ThreadId::AOCSThread),
         SenderReceiverThread::new(DeviceId::OBC, DeviceId::Camera, ThreadId::PLDThread),
         SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::PLDThread),
         SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::PLDThread),
@@ -306,6 +316,7 @@ pub fn load_package_id_to_apids() -> HashMap<PackageId, SenderReceiverApid> {
         SenderReceiverApid::new(DeviceId::OBC, DeviceId::SunSensor5, CSS_APID),
         SenderReceiverApid::new(DeviceId::OBC, DeviceId::SunSensor6, CSS_APID),
         SenderReceiverApid::new(DeviceId::OBC, DeviceId::StarTracker, STR_APID),
+        SenderReceiverApid::new(DeviceId::OBC, DeviceId::GPS, GPS_APID),
         SenderReceiverApid::new(DeviceId::MGM1, DeviceId::OBC, MGM_APID),
         SenderReceiverApid::new(DeviceId::MGM2, DeviceId::OBC, MGM_APID),
         SenderReceiverApid::new(DeviceId::MGM3, DeviceId::OBC, MGM_APID),
@@ -320,6 +331,9 @@ pub fn load_package_id_to_apids() -> HashMap<PackageId, SenderReceiverApid> {
         SenderReceiverApid::new(DeviceId::StarTracker, DeviceId::OBC, STR_APID),
         SenderReceiverApid::new(DeviceId::StarTracker, DeviceId::OBC, STR_APID),
         SenderReceiverApid::new(DeviceId::StarTracker, DeviceId::OBC, STR_APID),
+        SenderReceiverApid::new(DeviceId::GPS, DeviceId::OBC, GPS_APID),
+        SenderReceiverApid::new(DeviceId::GPS, DeviceId::OBC, GPS_APID),
+        SenderReceiverApid::new(DeviceId::GPS, DeviceId::OBC, GPS_APID),
         SenderReceiverApid::new(DeviceId::OBC, DeviceId::Camera, PLD_APID),
         SenderReceiverApid::new(DeviceId::Camera, DeviceId::OBC, PLD_APID),
         SenderReceiverApid::new(DeviceId::Camera, DeviceId::OBC, PLD_APID),

src/helpers.rs

@@ -15,6 +15,7 @@ use satrs_core::spacepackets::time::TimeWriter;
 use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
 use satrs_core::spacepackets::SpHeader;
 use std::sync::mpsc::Sender;
+use log::info;
 
 #[derive(Clone)]
 pub struct VerifHelper {
@@ -141,6 +142,7 @@ impl ModeHelper {
         &mut self,
         mode_submode: ModeAndSubmode,
     ) -> Result<(), std::sync::mpsc::SendError<StoreAddr>> {
+        info!("mode reply generated from apid {}", self.apid);
         self.update_time_stamp();
         let mut sp_header =
             SpHeader::tm_unseg(self.apid, self.seq_count_provider.get_and_increment(), 0).unwrap();

src/hk.rs

@@ -30,12 +30,12 @@ use strum_macros::EnumIter;
 
 pub type CollectionIntervalFactor = u32;
 
-pub const MGM_VOLTAGE_1: UniqueId = 1;
+pub const MGM_FIELD_1: UniqueId = 1;
-pub const MGM_VOLTAGE_2: UniqueId = 2;
+pub const MGM_FIELD_2: UniqueId = 2;
-pub const MGM_VOLTAGE_3: UniqueId = 3;
+pub const MGM_FIELD_3: UniqueId = 3;
-pub const CSS_VOLTAGE_1: UniqueId = 4;
+pub const CSS_SUN_VECTOR_1: UniqueId = 4;
-pub const CSS_VOLTAGE_2: UniqueId = 5;
+pub const CSS_SUN_VECTOR_2: UniqueId = 5;
-pub const CSS_VOLTAGE_3: UniqueId = 6;
+pub const CSS_SUN_VECTOR_3: UniqueId = 6;
 pub const CSS_VOLTAGE_4: UniqueId = 7;
 pub const CSS_VOLTAGE_5: UniqueId = 8;
 pub const CSS_VOLTAGE_6: UniqueId = 9;
@@ -43,8 +43,11 @@ pub const STR_QUATERNION_1: UniqueId = 10;
 pub const STR_QUATERNION_2: UniqueId = 11;
 pub const STR_QUATERNION_3: UniqueId = 12;
 pub const STR_QUATERNION_4: UniqueId = 13;
+pub const GPS_LATITUDE: UniqueId = 14;
+pub const GPS_LONGITUDE: UniqueId = 15;
+pub const GPS_ALTITUDE: UniqueId = 16;
 
-pub const AOCS_HK_NUM_OF_ELEMENTS: usize = 13 * 8;
+pub const AOCS_HK_NUM_OF_ELEMENTS: usize = 16 * 8;
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 pub enum AocsHkIds {
@@ -52,7 +55,7 @@ pub enum AocsHkIds {
     TestMgmSet = 2,
 }
 
-#[derive(Clone)]
+#[derive(Clone, Debug)]
 pub struct AocsDataMap {
     map: HashMap<UniqueId, AocsDataType>,
 }
@@ -61,12 +64,12 @@ impl AocsDataMap {
     pub fn new() -> Self {
         let mut data_map = HashMap::new();
 
-        data_map.insert(MGM_VOLTAGE_1, float_value(0.0));
+        data_map.insert(MGM_FIELD_1, float_value(0.0));
-        data_map.insert(MGM_VOLTAGE_2, float_value(0.0));
+        data_map.insert(MGM_FIELD_2, float_value(0.0));
-        data_map.insert(MGM_VOLTAGE_3, float_value(0.0));
+        data_map.insert(MGM_FIELD_3, float_value(0.0));
-        data_map.insert(CSS_VOLTAGE_1, float_value(0.0));
+        data_map.insert(CSS_SUN_VECTOR_1, float_value(0.0));
-        data_map.insert(CSS_VOLTAGE_2, float_value(0.0));
+        data_map.insert(CSS_SUN_VECTOR_2, float_value(0.0));
-        data_map.insert(CSS_VOLTAGE_3, float_value(0.0));
+        data_map.insert(CSS_SUN_VECTOR_3, float_value(0.0));
         data_map.insert(CSS_VOLTAGE_4, float_value(0.0));
         data_map.insert(CSS_VOLTAGE_5, float_value(0.0));
         data_map.insert(CSS_VOLTAGE_6, float_value(0.0));
@@ -74,6 +77,9 @@ impl AocsDataMap {
         data_map.insert(STR_QUATERNION_2, float_value(0.0));
         data_map.insert(STR_QUATERNION_3, float_value(0.0));
         data_map.insert(STR_QUATERNION_4, float_value(0.0));
+        data_map.insert(GPS_LATITUDE, float_value(0.0));
+        data_map.insert(GPS_LONGITUDE, float_value(0.0));
+        data_map.insert(GPS_ALTITUDE, float_value(0.0));
 
         Self { map: data_map }
     }
@@ -138,12 +144,12 @@ impl AocsHousekeeper {
         verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
     ) -> AocsHousekeeper {
         let id_list = vec![
-            MGM_VOLTAGE_1,
+            MGM_FIELD_1,
-            MGM_VOLTAGE_2,
+            MGM_FIELD_2,
-            MGM_VOLTAGE_3,
+            MGM_FIELD_3,
-            CSS_VOLTAGE_1,
+            CSS_SUN_VECTOR_1,
-            CSS_VOLTAGE_2,
+            CSS_SUN_VECTOR_2,
-            CSS_VOLTAGE_3,
+            CSS_SUN_VECTOR_3,
             CSS_VOLTAGE_4,
             CSS_VOLTAGE_5,
             CSS_VOLTAGE_6,
@@ -151,6 +157,9 @@ impl AocsHousekeeper {
             STR_QUATERNION_2,
             STR_QUATERNION_3,
             STR_QUATERNION_4,
+            GPS_LATITUDE,
+            GPS_LONGITUDE,
+            GPS_ALTITUDE,
         ];
 
         AocsHousekeeper {
@@ -178,12 +187,12 @@ impl AocsHousekeeper {
         collection_interval: Duration,
     ) -> AocsHousekeeper {
         let id_list = vec![
-            MGM_VOLTAGE_1,
+            MGM_FIELD_1,
-            MGM_VOLTAGE_2,
+            MGM_FIELD_2,
-            MGM_VOLTAGE_3,
+            MGM_FIELD_3,
-            CSS_VOLTAGE_1,
+            CSS_SUN_VECTOR_1,
-            CSS_VOLTAGE_2,
+            CSS_SUN_VECTOR_2,
-            CSS_VOLTAGE_3,
+            CSS_SUN_VECTOR_3,
             CSS_VOLTAGE_4,
             CSS_VOLTAGE_5,
             CSS_VOLTAGE_6,
@@ -191,6 +200,9 @@ impl AocsHousekeeper {
             STR_QUATERNION_2,
             STR_QUATERNION_3,
             STR_QUATERNION_4,
+            GPS_LATITUDE,
+            GPS_LONGITUDE,
+            GPS_ALTITUDE,
         ];
 
         AocsHousekeeper {
@@ -269,6 +281,7 @@ impl AocsHousekeeper {
             self.prev_time_step = TimeProvider::from_now_with_u16_days().unwrap();
             let map = self.data_map.lock().unwrap();
             let mut data_as_bytes = map.all_values_as_bytes();
+            debug!("{:?}", &map);
             drop(map);
             self.send_hk_packet(1, &mut data_as_bytes);
         }

src/main.rs

@@ -20,10 +20,7 @@ mod requests;
 mod tmtc;
 
 use crate::requests::RequestWithToken;
-use crate::tmtc::{
+use crate::tmtc::{core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, AOCS_APID, AOCS_HK_APID, CSS_APID, MGM_APID, MGT_APID, PLD_APID, PUS_APID, PWR_APID, RWL_APID, STR_APID, GPS_APID};
-    core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, AOCS_APID,
-    AOCS_HK_APID, CSS_APID, MGM_APID, MGT_APID, PLD_APID, PUS_APID, PWR_APID, RWL_APID, STR_APID,
-};
 use eurosim_obsw::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT};
 use satrs_core::event_man::{
     EventManagerWithMpscQueue, MpscEventReceiver, MpscEventU32SendProvider, SendEventProvider,
@@ -53,13 +50,13 @@ use std::sync::mpsc::channel;
 use std::sync::{mpsc, Arc, Mutex, RwLock};
 use std::thread;
 //use libc::time64_t;
-use crate::aocs::{core_aocs_loop, AocsController, MGMHandler};
+use crate::aocs::{core_aocs_loop, AocsController, MGMHandler, STRHandler, GPSHandler, CSSHandler};
 #[cfg(feature = "can")]
 use crate::can::CanTxHandler;
 use crate::helpers::{ModeHelper, VerifHelper};
 use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper};
 use crate::pld_handler::core_pld_task;
-use crate::power_handler::{core_power_task, PowerSwitcher};
+use crate::power_handler::{core_power_task, DeviceState, PowerSwitcher};
 
 fn main() {
     logger::setup_logger().unwrap();
@@ -90,7 +87,12 @@ fn main() {
 
     let seq_count_provider = SeqCountProviderSyncClonable::default();
     let msg_count_provider = SeqCountProviderSyncClonable::default();
-    let aocs_seq_count_provider = SeqCountProviderSyncClonable::default();
+    let aocs_hk_seq_count_provider = SeqCountProviderSyncClonable::default();
+    let aocs_ctrl_seq_count_provider = SeqCountProviderSyncClonable::default();
+    let mgm_seq_count_provider = SeqCountProviderSyncClonable::default();
+    let css_seq_count_provider = SeqCountProviderSyncClonable::default();
+    let str_seq_count_provider = SeqCountProviderSyncClonable::default();
+    let gps_seq_count_provider = SeqCountProviderSyncClonable::default();
     let verif_seq_count_provider = seq_count_provider.clone();
     let tmtc_seq_count_provider = seq_count_provider.clone();
 
@@ -135,6 +137,7 @@ fn main() {
     let (mgm_tx, mgm_rx) = channel::<RequestWithToken>();
     let (css_tx, css_rx) = channel::<RequestWithToken>();
     let (str_tx, str_rx) = channel::<RequestWithToken>();
+    let (gps_tx, gps_rx) = channel::<RequestWithToken>();
     let (mgt_tx, mgt_rx) = channel::<RequestWithToken>();
     let (rwl_tx, rwl_rx) = channel::<RequestWithToken>();
     request_map.insert(RequestTargetId::AcsSubsystem as u32, aocs_thread_tx.clone());
@@ -148,7 +151,6 @@ fn main() {
     };
 
     let mut apid_map = HashMap::new();
-
     apid_map.insert(PLD_APID, pld_thread_tx.clone());
     apid_map.insert(PWR_APID, pwr_thread_tx.clone());
     apid_map.insert(AOCS_APID, aocs_thread_tx.clone());
@@ -156,6 +158,7 @@ fn main() {
     apid_map.insert(MGM_APID, mgm_tx.clone());
     apid_map.insert(CSS_APID, css_tx.clone());
     apid_map.insert(STR_APID, str_tx.clone());
+    apid_map.insert(GPS_APID, gps_tx.clone());
     apid_map.insert(MGT_APID, mgt_tx.clone());
     apid_map.insert(RWL_APID, rwl_tx.clone());
 
@@ -184,6 +187,7 @@ fn main() {
     let (css_can_tx, css_can_rx) = mpsc::channel::<PackageModel>();
     let (rwl_can_tx, rwl_can_rx) = mpsc::channel::<PackageModel>();
     let (str_can_tx, str_can_rx) = mpsc::channel::<PackageModel>();
+    let (gps_can_tx, gps_can_rx) = mpsc::channel::<PackageModel>();
     let (power_can_tx, power_can_rx) = mpsc::channel::<PackageModel>();
     let (pld_can_tx, pld_can_rx) = mpsc::channel::<PackageModel>();
 
@@ -196,6 +200,7 @@ fn main() {
     can_senders.insert(RWL_APID, rwl_can_tx);
     can_senders.insert(CSS_APID, css_can_tx);
     can_senders.insert(STR_APID, str_can_tx);
+    can_senders.insert(GPS_APID, gps_can_tx);
 
     // get package id hashmap
     let package_ids_rx = load_package_id_to_apids();
@@ -297,6 +302,7 @@ fn main() {
     let aocs_tm_store = tm_store.clone();
 
     let mut aocs_data_not_threadsafe = AocsDataMap::new();
+    /*
     aocs_data_not_threadsafe
         .update_value(1, AocsDataType::float_value(1.0))
         .unwrap();
@@ -336,73 +342,125 @@ fn main() {
     aocs_data_not_threadsafe
         .update_value(13, AocsDataType::float_value(13.0))
         .unwrap();
+     */
     let aocs_data = Arc::new(Mutex::new(aocs_data_not_threadsafe));
 
-    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 aocs_housekeeper = AocsHousekeeper::new_with_collection_interval(
             aocs_data.clone(),
             aocs_hk_rx,
-            aocs_seq_count_provider.clone(),
+            aocs_hk_seq_count_provider,
             aocs_tm_store.clone(),
             aocs_tm_funnel_tx.clone(),
             reporter_aocs.clone(),
             Duration::seconds(1),
         );
 
-        let aocs_controller_verif_helper = VerifHelper::new(verif_reporter.clone());
+        let aocs_verif_helper = VerifHelper::new(verif_reporter.clone());
         let aocs_controller_mode_helper = ModeHelper::new(
             AOCS_APID,
-            aocs_seq_count_provider.clone(),
+            aocs_ctrl_seq_count_provider.clone(),
             aocs_tm_store.clone(),
             aocs_tm_funnel_tx.clone(),
         );
 
         let mut aocs_controller = AocsController::new(
             aocs_thread_rx,
-            mgm_tx,
+            mgm_tx.clone(),
-            css_tx,
+            css_tx.clone(),
-            str_tx,
+            str_tx.clone(),
-            aocs_seq_count_provider.clone(),
+            gps_tx.clone(),
-            aocs_controller_verif_helper,
+            aocs_ctrl_seq_count_provider.clone(),
+            aocs_verif_helper.clone(),
             aocs_controller_mode_helper,
         );
 
+        let mgm_mode_helper = ModeHelper::new(
+            MGM_APID,
+            mgm_seq_count_provider,
+            aocs_tm_store.clone(),
+            aocs_tm_funnel_tx.clone(),
+        );
+
+        let mut mgm_handler = MGMHandler::new(
+            power_switcher.clone(),
+            DeviceId::MGM1,
+            DeviceState::Off,
+            aocs_data.clone(),
+            can_tx_sender.clone(),
+            mgm_can_rx,
+            mgm_rx,
+            aocs_verif_helper.clone(),
+            mgm_mode_helper,
+        );
+
+        let str_mode_helper = ModeHelper::new(
+            STR_APID,
+            str_seq_count_provider,
+            aocs_tm_store.clone(),
+            aocs_tm_funnel_tx.clone(),
+        );
+
+        let mut str_handler = STRHandler::new(
+            power_switcher.clone(),
+            DeviceId::StarTracker,
+            DeviceState::Off,
+            aocs_data.clone(),
+            can_tx_sender.clone(),
+            str_can_rx,
+            str_rx,
+            aocs_verif_helper.clone(),
+            str_mode_helper,
+        );
+
+        let css_mode_helper = ModeHelper::new(
+            CSS_APID,
+            css_seq_count_provider,
+            aocs_tm_store.clone(),
+            aocs_tm_funnel_tx.clone(),
+        );
+
+        let mut css_handler = CSSHandler::new(
+            power_switcher.clone(),
+            DeviceId::SunSensor1,
+            DeviceState::Off,
+            aocs_data.clone(),
+            can_tx_sender.clone(),
+            css_can_rx,
+            css_rx,
+            aocs_verif_helper.clone(),
+            css_mode_helper,
+        );
+
+        let gps_mode_helper = ModeHelper::new(
+            GPS_APID,
+            gps_seq_count_provider,
+            aocs_tm_store.clone(),
+            aocs_tm_funnel_tx.clone(),
+        );
+
+        let mut gps_handler = GPSHandler::new(
+            power_switcher.clone(),
+            DeviceId::GPS,
+            DeviceState::Off,
+            aocs_data.clone(),
+            can_tx_sender.clone(),
+            gps_can_rx,
+            gps_rx,
+            aocs_verif_helper.clone(),
+            gps_mode_helper,
+        );
+
         loop {
             aocs_housekeeper.periodic_op();
             aocs_controller.periodic_op();
-        }
-        /*let mut mgm_handler = MGMHandler::new(
-            power_switcher_aocs.clone(),
-            DeviceId::MGM1,
-            can_tx_sender.clone(),
-            mgm_can_rx,
-            mgm_action_rx,
-        );
-
-        let aocs_sensor_data = Arc::new(Mutex::new(AocsDataMap::new()));
-        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 {
             mgm_handler.periodic_op();
-            let mut locked_sensor_data = aocs_sensor_data.lock().unwrap();
+            str_handler.periodic_op();
-            locked_sensor_data.update_mgm_data(&mgm_handler.get_data_ref());
+            css_handler.periodic_op();
-            drop(locked_sensor_data);
+            gps_handler.periodic_op();
-            aocs_housekeeper.handle_hk_request();
         }
-
-         */
-
-        core_aocs_loop();
     });
 
     jh0.unwrap()

src/power_handler.rs

@@ -108,8 +108,6 @@ impl PCDU {
             let mut map_lock = self.device_state_map.lock().unwrap();
             *map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
             // TODO: potentially change bus logic -> remove acceptance and verification of power off/on, since status is simply called in next step anyway
-            self.can_rx.recv().unwrap();
-            self.can_rx.recv().unwrap();
             Ok(())
         } else {
             Err(())
@@ -125,8 +123,6 @@ impl PCDU {
                 .unwrap();
             let mut map_lock = self.device_state_map.lock().unwrap();
             *map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
-            self.can_rx.recv().unwrap();
-            self.can_rx.recv().unwrap();
             Ok(())
         } else {
             Err(())
@@ -134,7 +130,7 @@ impl PCDU {
     }
 
     pub fn update_states_helper(&mut self, dev_id: &DeviceId) -> Result<(), ()> {
-        let _switch_id: SwitchId = *dev_id as u16;
+        /*let _switch_id: SwitchId = *dev_id as u16;
         let dev_id_bytes = *dev_id as u8;
         let buf: &[u8] = &dev_id_bytes.to_be_bytes();
         self.can_tx
@@ -165,6 +161,9 @@ impl PCDU {
             }
             Err(_) => Err(()),
         }
+
+         */
+        Ok(())
     }
 
     pub fn update_all_states_helper(&mut self) -> Result<(), ()> {
@@ -180,11 +179,18 @@ impl PCDU {
         Ok(())
     }
 
-    pub fn update_switch_states(&mut self, switch_id: SwitchId) -> Result<(), ()> {
+    pub fn update_switch_states(&mut self, switch_id: SwitchId, mut switch_state: SwitchState) -> Result<(), ()> {
         return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
             match dev_id {
-                DeviceId::All => self.update_all_states_helper(),
+                //DeviceId::All => self.update_all_states_helper(),
-                _ => self.update_states_helper(&dev_id),
+                DeviceId::All => {Ok(())}
+                _ => {
+                    let mut map = self.device_state_map.lock().expect("error locking data map");
+                    if let Some(mut val) = map.get_mut(&dev_id) {
+                        val = &mut switch_state;
+                    }
+                    Ok(())
+                },
             }
         } else {
             Err(())
@@ -197,6 +203,7 @@ impl PCDU {
             match switch_state {
                 SwitchState::Off => match self.send_power_off(switch_id) {
                     Ok(_) => {
+                        self.update_switch_states(switch_id, switch_state).expect("error updating switch states");
                         i = i + 1;
                     }
                     Err(_) => {
@@ -205,6 +212,7 @@ impl PCDU {
                 },
                 SwitchState::On => match self.send_power_on(switch_id) {
                     Ok(_) => {
+                        self.update_switch_states(switch_id, switch_state).expect("error updating switch states");
                         i = i + 1;
                     }
                     Err(_) => {
@@ -213,7 +221,6 @@ impl PCDU {
                 },
                 _ => {}
             }
-            self.update_switch_states(switch_id).unwrap();
         }
         return Ok(i);
     }

src/pus.rs

@@ -596,7 +596,7 @@ impl PusReceiver {
         }
 
          */
-        let app_data = app_data.unwrap();
+
         let mut invalid_subservice_handler = || {
             self.tm_args
                 .verif_reporter
@@ -650,17 +650,21 @@ impl PusReceiver {
                             )
                             .expect("Sending start failure TM failed");
                     }
-                    // Should never fail after size check
+                    if let Some(app_data) = app_data {
-                    let mode_submode = ModeAndSubmode::from_be_bytes(
+                        // Should never fail after size check
-                        app_data[0..ModeAndSubmode::raw_len()].try_into().unwrap(),
+                        let mode_submode = ModeAndSubmode::from_be_bytes(
-                    )
+                            app_data[0..ModeAndSubmode::raw_len()].try_into().unwrap(),
-                    .unwrap();
+                        )
-                    info!("{:?}", mode_submode);
+                            .unwrap();
-                    //forward_mode_request(target_id, ModeRequest::SetMode(mode_submode));
+                        info!("{:?}", mode_submode);
-                    Some(RequestWithToken(
+                        //forward_mode_request(target_id, ModeRequest::SetMode(mode_submode));
-                        Request::ModeRequest(ModeRequest::SetMode(mode_submode)),
+                        Some(RequestWithToken(
-                        Some(token),
+                            Request::ModeRequest(ModeRequest::SetMode(mode_submode)),
-                    ))
+                            Some(token),
+                        ))
+                    } else {
+                        None
+                    }
                 }
                 Subservice::TcReadMode => {
                     //let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());

src/tmtc.rs

@@ -36,6 +36,7 @@ pub const CSS_APID: u16 = 0x53;
 pub const STR_APID: u16 = 0x54;
 pub const MGT_APID: u16 = 0x55;
 pub const RWL_APID: u16 = 0x56;
+pub const GPS_APID: u16 = 0x57;
 
 pub struct OtherArgs {
     pub sock_addr: SocketAddr,