422 lines
15 KiB
Rust
422 lines
15 KiB
Rust
use std::collections::HashMap;
|
|
|
|
use crate::hk::AocsDataType::FloatValue;
|
|
use crate::requests::Request;
|
|
use crate::requests::RequestWithToken;
|
|
use crate::tmtc::{TmStore, AOCS_HK_APID};
|
|
use byteorder::{BigEndian, ByteOrder, LittleEndian};
|
|
use chrono::Duration;
|
|
use eurosim_obsw::hk_err;
|
|
use log::debug;
|
|
use satrs_core::hk::{HkRequest, UniqueId};
|
|
use satrs_core::pool::StoreAddr;
|
|
use satrs_core::pus::hk::Subservice;
|
|
use satrs_core::pus::verification::{FailParams, VerificationReporterWithSender};
|
|
use satrs_core::pus::MpscPusInStoreSendError;
|
|
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
|
|
use satrs_core::spacepackets::time::cds::{TimeProvider};
|
|
use satrs_core::spacepackets::time::{CcsdsTimeProvider, TimeWriter};
|
|
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
|
|
use satrs_core::spacepackets::SpHeader;
|
|
use std::sync::mpsc::{Receiver, Sender};
|
|
use std::sync::{Arc, Mutex};
|
|
use strum_macros::EnumIter;
|
|
|
|
pub type CollectionIntervalFactor = u32;
|
|
|
|
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;
|
|
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 = 16 * 8;
|
|
|
|
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
|
|
pub enum AocsHkIds {
|
|
TestAocsSet = 1,
|
|
TestMgmSet = 2,
|
|
}
|
|
|
|
#[derive(Clone, Debug)]
|
|
pub struct AocsDataMap {
|
|
map: HashMap<UniqueId, AocsDataType>,
|
|
}
|
|
|
|
impl AocsDataMap {
|
|
pub fn new() -> Self {
|
|
let mut data_map = HashMap::new();
|
|
|
|
data_map.insert(MGM_FIELD_1, FloatValue(0.0));
|
|
data_map.insert(MGM_FIELD_2, FloatValue(0.0));
|
|
data_map.insert(MGM_FIELD_3, FloatValue(0.0));
|
|
data_map.insert(CSS_SUN_VECTOR_1, FloatValue(0.0));
|
|
data_map.insert(CSS_SUN_VECTOR_2, FloatValue(0.0));
|
|
data_map.insert(CSS_SUN_VECTOR_3, FloatValue(0.0));
|
|
data_map.insert(CSS_VOLTAGE_4, FloatValue(0.0));
|
|
data_map.insert(CSS_VOLTAGE_5, FloatValue(0.0));
|
|
data_map.insert(CSS_VOLTAGE_6, FloatValue(0.0));
|
|
data_map.insert(STR_QUATERNION_1, FloatValue(0.0));
|
|
data_map.insert(STR_QUATERNION_2, FloatValue(0.0));
|
|
data_map.insert(STR_QUATERNION_3, FloatValue(0.0));
|
|
data_map.insert(STR_QUATERNION_4, FloatValue(0.0));
|
|
data_map.insert(GPS_LATITUDE, FloatValue(0.0));
|
|
data_map.insert(GPS_LONGITUDE, FloatValue(0.0));
|
|
data_map.insert(GPS_ALTITUDE, FloatValue(0.0));
|
|
|
|
Self { map: data_map }
|
|
}
|
|
|
|
pub fn update_value(&mut self, id: UniqueId, val: AocsDataType) -> Result<(), ()> {
|
|
if self.map.contains_key(&id) {
|
|
self.map.insert(id, val);
|
|
return Ok(());
|
|
}
|
|
Err(())
|
|
}
|
|
|
|
pub fn get_value(&self, id: UniqueId) -> Option<&AocsDataType> {
|
|
self.map.get(&id)
|
|
}
|
|
|
|
pub fn all_values_as_bytes(&self) -> [u8; AOCS_HK_NUM_OF_ELEMENTS] {
|
|
let mut buf: [u8; AOCS_HK_NUM_OF_ELEMENTS] = [0; AOCS_HK_NUM_OF_ELEMENTS];
|
|
let mut i = 0;
|
|
let map = self.map.clone();
|
|
let mut map_as_vec = map.keys().collect::<Vec<&UniqueId>>();
|
|
map_as_vec.sort();
|
|
for element in map_as_vec {
|
|
match map.get(element).unwrap() {
|
|
FloatValue(val) => {
|
|
let val_as_byte = BigEndian::write_f64(&mut (buf[i..i + 8]), *val);
|
|
}
|
|
}
|
|
i += 8;
|
|
}
|
|
debug!("{:?}", buf);
|
|
buf
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, EnumIter, PartialEq, Copy, Clone)]
|
|
pub enum AocsDataType {
|
|
FloatValue(f64),
|
|
}
|
|
|
|
pub struct AocsHousekeeper {
|
|
data_map: Arc<Mutex<AocsDataMap>>,
|
|
id_list: Vec<UniqueId>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
seq_count_provider: SeqCountProviderSyncClonable,
|
|
aocs_tm_store: TmStore,
|
|
aocs_tm_funnel_tx: Sender<StoreAddr>,
|
|
verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
|
|
periodic_hk_ids: Option<Vec<UniqueId>>,
|
|
periodic_on: bool,
|
|
prev_time_step: TimeProvider,
|
|
collection_interval: Duration,
|
|
}
|
|
|
|
impl AocsHousekeeper {
|
|
pub fn new(
|
|
sensor_data_pool: Arc<Mutex<AocsDataMap>>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
seq_count_provider: SeqCountProviderSyncClonable,
|
|
aocs_tm_store: TmStore,
|
|
aocs_tm_funnel_tx: Sender<StoreAddr>,
|
|
verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
|
|
) -> AocsHousekeeper {
|
|
let id_list = vec![
|
|
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,
|
|
STR_QUATERNION_1,
|
|
STR_QUATERNION_2,
|
|
STR_QUATERNION_3,
|
|
STR_QUATERNION_4,
|
|
GPS_LATITUDE,
|
|
GPS_LONGITUDE,
|
|
GPS_ALTITUDE,
|
|
];
|
|
|
|
AocsHousekeeper {
|
|
data_map: sensor_data_pool,
|
|
id_list,
|
|
request_rx,
|
|
seq_count_provider,
|
|
aocs_tm_store,
|
|
aocs_tm_funnel_tx,
|
|
verif_reporter,
|
|
periodic_hk_ids: None,
|
|
periodic_on: false,
|
|
prev_time_step: TimeProvider::from_now_with_u16_days().unwrap(),
|
|
collection_interval: Duration::seconds(0),
|
|
}
|
|
}
|
|
|
|
pub fn new_with_collection_interval(
|
|
sensor_data_pool: Arc<Mutex<AocsDataMap>>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
seq_count_provider: SeqCountProviderSyncClonable,
|
|
aocs_tm_store: TmStore,
|
|
aocs_tm_funnel_tx: Sender<StoreAddr>,
|
|
verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
|
|
collection_interval: Duration,
|
|
) -> AocsHousekeeper {
|
|
let id_list = vec![
|
|
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,
|
|
STR_QUATERNION_1,
|
|
STR_QUATERNION_2,
|
|
STR_QUATERNION_3,
|
|
STR_QUATERNION_4,
|
|
GPS_LATITUDE,
|
|
GPS_LONGITUDE,
|
|
GPS_ALTITUDE,
|
|
];
|
|
|
|
AocsHousekeeper {
|
|
data_map: sensor_data_pool,
|
|
id_list,
|
|
request_rx,
|
|
seq_count_provider,
|
|
aocs_tm_store,
|
|
aocs_tm_funnel_tx,
|
|
verif_reporter,
|
|
periodic_hk_ids: None,
|
|
periodic_on: false,
|
|
prev_time_step: TimeProvider::from_now_with_u16_days().unwrap(),
|
|
collection_interval,
|
|
}
|
|
}
|
|
|
|
pub fn periodic_op(&mut self) {
|
|
self.handle_hk_request();
|
|
self.periodic_hk();
|
|
}
|
|
|
|
// housekeeper has methods for both sending individual variables as tm as well as entire aocs data set
|
|
// individual values results in way too much traffic, therefore is not used
|
|
// the functions for it are kept here since the logic could be easily adapted to send different sets (something which is currently not implemented)
|
|
// for now, ..._individual functions are rather unnecessary
|
|
pub fn handle_hk_request(&mut self) {
|
|
let mut time_stamp_buf: [u8; 7] = [0; 7];
|
|
|
|
if let Ok(request_with_token) = self.request_rx.try_recv() {
|
|
if let Request::HkRequest(hk_req) = request_with_token.0 {
|
|
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
|
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
|
|
let start_token = self //implement this for verification
|
|
.verif_reporter
|
|
.start_success(request_with_token.1.unwrap(), Some(&time_stamp_buf))
|
|
.expect("Error sending start success");
|
|
if let Ok(()) = match hk_req {
|
|
HkRequest::OneShot(id) => self.one_shot(),
|
|
HkRequest::Enable(id) => self.enable_periodic(),
|
|
HkRequest::Disable(id) => self.disable_periodic(),
|
|
HkRequest::ModifyCollectionInterval(_id, _collection_interval) => Ok(()),
|
|
} {
|
|
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
|
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
|
|
self.verif_reporter
|
|
.completion_success(start_token, Some(&time_stamp_buf))
|
|
.expect("Error sending completion success");
|
|
} else {
|
|
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
|
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
|
|
self.verif_reporter
|
|
.completion_failure(
|
|
start_token,
|
|
FailParams::new(
|
|
Some(&time_stamp_buf),
|
|
&hk_err::UNKNOWN_TARGET_ID,
|
|
None,
|
|
),
|
|
)
|
|
.expect("Error sending completion success");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn check_period(&mut self) -> bool {
|
|
let current_time = TimeProvider::from_now_with_u16_days().unwrap();
|
|
let prev_time = self.prev_time_step.clone();
|
|
let delta = current_time.date_time().unwrap() - prev_time.date_time().unwrap();
|
|
delta > self.collection_interval
|
|
}
|
|
|
|
pub fn periodic_hk(&mut self) {
|
|
if self.periodic_on && self.check_period() {
|
|
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);
|
|
}
|
|
}
|
|
|
|
pub fn periodic_hk_individual(&mut self) {
|
|
//let json_string = self.aocs_data_to_str();
|
|
let data = self.data_map.lock().unwrap();
|
|
let data_copy = data.clone();
|
|
drop(data);
|
|
let mut buf: [u8; 8] = [0; 8];
|
|
if let Some(ids) = &self.periodic_hk_ids {
|
|
for id in ids.clone() {
|
|
if let Some(FloatValue(field)) = data_copy.get_value(id) {
|
|
let data_str = LittleEndian::write_f64(&mut buf, *field);
|
|
let _ = &self.send_hk_packet(id, &buf);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn one_shot(&mut self) -> Result<(), ()> {
|
|
let map = self.data_map.lock().unwrap();
|
|
let data_as_bytes = map.all_values_as_bytes();
|
|
drop(map);
|
|
// currently gives 1 as id, meaning one is the spid, if multiple spids in this housekeeper, this needs to change!
|
|
self.send_hk_packet(1, &data_as_bytes);
|
|
Ok(())
|
|
}
|
|
|
|
pub fn one_shot_hk_individual(&mut self, id: UniqueId) -> Result<(), ()> {
|
|
let data = self.data_map.lock().unwrap();
|
|
let data_copy = data.clone();
|
|
drop(data);
|
|
let mut buf: [u8; 8] = [0; 8];
|
|
if let Some(FloatValue(field)) = data_copy.get_value(id) {
|
|
let data_str = LittleEndian::write_f64(&mut buf, *field);
|
|
self.send_hk_packet(id, &buf);
|
|
return Ok(());
|
|
}
|
|
Err(())
|
|
}
|
|
|
|
pub fn enable_periodic(&mut self) -> Result<(), ()> {
|
|
self.periodic_on = true;
|
|
Ok(())
|
|
}
|
|
|
|
pub fn enable_periodic_individual(&mut self, id: UniqueId) -> Result<(), ()> {
|
|
if !self.id_list.contains(&id) {
|
|
return Err(());
|
|
}
|
|
|
|
let periodic_hk_ids = self.periodic_hk_ids.clone();
|
|
match periodic_hk_ids {
|
|
None => self.periodic_hk_ids = Some(vec![id]),
|
|
Some(mut ids) => {
|
|
ids.push(id);
|
|
self.periodic_hk_ids = Some(ids);
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
pub fn disable_periodic(&mut self) -> Result<(), ()> {
|
|
self.periodic_on = false;
|
|
Ok(())
|
|
}
|
|
|
|
pub fn disable_periodic_individual(&mut self, id: UniqueId) -> Result<(), ()> {
|
|
if !self.id_list.contains(&id) {
|
|
return Err(());
|
|
}
|
|
|
|
let periodic_hk_ids = self.periodic_hk_ids.clone();
|
|
match periodic_hk_ids {
|
|
None => return Err(()),
|
|
Some(mut ids) => {
|
|
if !ids.contains(&id) {
|
|
return Err(());
|
|
}
|
|
// .unwrap is allowed, since existence check is performed
|
|
let index = ids.iter().position(|x| *x == id).unwrap();
|
|
ids.remove(index);
|
|
if ids.len() < 1 {
|
|
self.periodic_hk_ids = None;
|
|
} else {
|
|
self.periodic_hk_ids = Some(ids);
|
|
}
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
/*
|
|
pub fn aocs_data_to_str(&mut self) -> String {
|
|
let pool = self.data_map.lock().unwrap();
|
|
serde_json::to_string(pool.deref()).unwrap()
|
|
}
|
|
|
|
*/
|
|
|
|
pub fn send_hk_packet_from_str(&mut self, id: UniqueId, data: &str) {
|
|
let mut time_stamp_buf: [u8; 7] = [0; 7];
|
|
let mut huge_buf: [u8; 8192] = [0; 8192];
|
|
|
|
let mut sp_header =
|
|
SpHeader::tm_unseg(0x02, self.seq_count_provider.get_and_increment(), 0).unwrap();
|
|
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
|
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
|
|
huge_buf[0..4].copy_from_slice(&id.to_be_bytes());
|
|
let mut len = 4;
|
|
huge_buf[8..data.len() + 8].copy_from_slice(data.as_bytes());
|
|
len += data.len();
|
|
let tm_sec_header =
|
|
PusTmSecondaryHeader::new_simple(3, Subservice::TmHkPacket as u8, &time_stamp_buf);
|
|
let hk_tm = PusTm::new(&mut sp_header, tm_sec_header, Some(&huge_buf[0..len]), true);
|
|
let addr = self.aocs_tm_store.add_pus_tm(&hk_tm);
|
|
self.aocs_tm_funnel_tx.send(addr).expect("sending failed");
|
|
}
|
|
|
|
pub fn send_hk_packet(&mut self, id: UniqueId, data: &[u8]) {
|
|
let mut time_stamp_buf: [u8; 7] = [0; 7];
|
|
let mut huge_buf: [u8; 8192] = [0; 8192];
|
|
|
|
let mut sp_header =
|
|
SpHeader::tm_unseg(AOCS_HK_APID, self.seq_count_provider.get_and_increment(), 0)
|
|
.unwrap();
|
|
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
|
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
|
|
huge_buf[0..4].copy_from_slice(&id.to_be_bytes());
|
|
let mut len = 8;
|
|
huge_buf[len..data.len() + 8].copy_from_slice(data);
|
|
len += data.len();
|
|
let data = huge_buf[0..len].to_vec();
|
|
let tm_sec_header =
|
|
PusTmSecondaryHeader::new_simple(3, Subservice::TmHkPacket as u8, &time_stamp_buf);
|
|
sp_header.data_len = len as u16;
|
|
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");
|
|
}
|
|
}
|