diff --git a/src/aocs.rs b/src/aocs.rs index a0d4746..94dbab6 100644 --- a/src/aocs.rs +++ b/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 => { - map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data)) + PackageId::AOCSDataMGM1 => { + map.update_value(MGM_FIELD_1, AocsDataType::float_value(float_data)) .expect("error updating value"); } - PackageId::AOCSDataStarTracker2 => { - map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data)) + PackageId::AOCSDataMGM2 => { + map.update_value(MGM_FIELD_2, AocsDataType::float_value(float_data)) .expect("error updating value"); } - PackageId::AOCSDataStarTracker3 => { - 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)) + PackageId::AOCSDataMGM3 => { + map.update_value(MGM_FIELD_3, 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 => { - map.update_value(MGM_VOLTAGE_1, AocsDataType::float_value(float_data)) + PackageId::AOCSDataStarTracker1 => { + map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data)) .expect("error updating value"); } - PackageId::AOCSDataMGM2 => { - map.update_value(MGM_VOLTAGE_2, AocsDataType::float_value(float_data)) + PackageId::AOCSDataStarTracker2 => { + map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data)) .expect("error updating value"); } - PackageId::AOCSDataMGM2 => { - map.update_value(MGM_VOLTAGE_3, AocsDataType::float_value(float_data)) + PackageId::AOCSDataStarTracker3 => { + 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>, + can_tx: Sender, + can_rx: Receiver, + request_rx: Receiver, + verif_helper: VerifHelper, + mode_helper: ModeHelper, +} + +impl AocsSensorHandler for GPSHandler { + type Error = (); + + fn get_package_id(&mut self) -> Result { + 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>, + can_tx: Sender, + can_rx: Receiver, + request_rx: Receiver, + 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, css_request_tx: Sender, str_request_tx: Sender, + gps_request_tx: Sender, seq_count_provider: SeqCountProviderSyncClonable, verif_helper: VerifHelper, mode_helper: ModeHelper, @@ -632,6 +856,7 @@ impl AocsController { mgm_request_tx: Sender, css_request_tx: Sender, str_request_tx: Sender, + gps_request_tx: Sender, 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 diff --git a/src/can_ids.rs b/src/can_ids.rs index e5cadfc..819366a 100644 --- a/src/can_ids.rs +++ b/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 for DeviceId { @@ -122,6 +127,7 @@ impl TryFrom 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 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 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 { 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 { 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), diff --git a/src/helpers.rs b/src/helpers.rs index 88dcccc..d13c394 100644 --- a/src/helpers.rs +++ b/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> { + 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(); diff --git a/src/hk.rs b/src/hk.rs index 7517b2c..1217fd1 100644 --- a/src/hk.rs +++ b/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_VOLTAGE_2: UniqueId = 2; -pub const MGM_VOLTAGE_3: UniqueId = 3; -pub const CSS_VOLTAGE_1: UniqueId = 4; -pub const CSS_VOLTAGE_2: UniqueId = 5; -pub const CSS_VOLTAGE_3: UniqueId = 6; +pub const MGM_FIELD_1: UniqueId = 1; +pub const MGM_FIELD_2: UniqueId = 2; +pub const MGM_FIELD_3: UniqueId = 3; +pub const CSS_SUN_VECTOR_1: UniqueId = 4; +pub const CSS_SUN_VECTOR_2: UniqueId = 5; +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, } @@ -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_VOLTAGE_2, float_value(0.0)); - data_map.insert(MGM_VOLTAGE_3, float_value(0.0)); - data_map.insert(CSS_VOLTAGE_1, float_value(0.0)); - data_map.insert(CSS_VOLTAGE_2, float_value(0.0)); - data_map.insert(CSS_VOLTAGE_3, float_value(0.0)); + data_map.insert(MGM_FIELD_1, float_value(0.0)); + data_map.insert(MGM_FIELD_2, float_value(0.0)); + data_map.insert(MGM_FIELD_3, float_value(0.0)); + data_map.insert(CSS_SUN_VECTOR_1, float_value(0.0)); + data_map.insert(CSS_SUN_VECTOR_2, 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, ) -> AocsHousekeeper { let id_list = vec![ - MGM_VOLTAGE_1, - MGM_VOLTAGE_2, - MGM_VOLTAGE_3, - CSS_VOLTAGE_1, - CSS_VOLTAGE_2, - CSS_VOLTAGE_3, + MGM_FIELD_1, + MGM_FIELD_2, + MGM_FIELD_3, + CSS_SUN_VECTOR_1, + CSS_SUN_VECTOR_2, + 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_VOLTAGE_2, - MGM_VOLTAGE_3, - CSS_VOLTAGE_1, - CSS_VOLTAGE_2, - CSS_VOLTAGE_3, + MGM_FIELD_1, + MGM_FIELD_2, + MGM_FIELD_3, + CSS_SUN_VECTOR_1, + CSS_SUN_VECTOR_2, + 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); } diff --git a/src/main.rs b/src/main.rs index d1f2d24..d52591c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -20,10 +20,7 @@ mod requests; mod tmtc; use crate::requests::RequestWithToken; -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, -}; +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}; 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::(); let (css_tx, css_rx) = channel::(); let (str_tx, str_rx) = channel::(); + let (gps_tx, gps_rx) = channel::(); let (mgt_tx, mgt_rx) = channel::(); let (rwl_tx, rwl_rx) = channel::(); 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::(); let (rwl_can_tx, rwl_can_rx) = mpsc::channel::(); let (str_can_tx, str_can_rx) = mpsc::channel::(); + let (gps_can_tx, gps_can_rx) = mpsc::channel::(); let (power_can_tx, power_can_rx) = mpsc::channel::(); let (pld_can_tx, pld_can_rx) = mpsc::channel::(); @@ -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, - css_tx, - str_tx, - aocs_seq_count_provider.clone(), - aocs_controller_verif_helper, + mgm_tx.clone(), + css_tx.clone(), + str_tx.clone(), + gps_tx.clone(), + 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(); - locked_sensor_data.update_mgm_data(&mgm_handler.get_data_ref()); - drop(locked_sensor_data); - aocs_housekeeper.handle_hk_request(); + str_handler.periodic_op(); + css_handler.periodic_op(); + gps_handler.periodic_op(); } - - */ - - core_aocs_loop(); }); jh0.unwrap() diff --git a/src/power_handler.rs b/src/power_handler.rs index 9ebf96c..3d0bfef 100644 --- a/src/power_handler.rs +++ b/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(), - _ => self.update_states_helper(&dev_id), + //DeviceId::All => self.update_all_states_helper(), + 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); } diff --git a/src/pus.rs b/src/pus.rs index c3126e4..ea0eb96 100644 --- a/src/pus.rs +++ b/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 - let mode_submode = ModeAndSubmode::from_be_bytes( - app_data[0..ModeAndSubmode::raw_len()].try_into().unwrap(), - ) - .unwrap(); - info!("{:?}", mode_submode); - //forward_mode_request(target_id, ModeRequest::SetMode(mode_submode)); - Some(RequestWithToken( - Request::ModeRequest(ModeRequest::SetMode(mode_submode)), - Some(token), - )) + if let Some(app_data) = app_data { + // Should never fail after size check + let mode_submode = ModeAndSubmode::from_be_bytes( + app_data[0..ModeAndSubmode::raw_len()].try_into().unwrap(), + ) + .unwrap(); + info!("{:?}", mode_submode); + //forward_mode_request(target_id, ModeRequest::SetMode(mode_submode)); + Some(RequestWithToken( + 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()); diff --git a/src/tmtc.rs b/src/tmtc.rs index e1e9689..5c7ac76 100644 --- a/src/tmtc.rs +++ b/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,