eurosim-obsw/src/hk.rs

use crate::aocs::{CSSData, MGMData};
use crate::requests::Request;
use crate::requests::RequestWithToken;
use crate::tmtc::TmStore;
use eurosim_obsw::{hk_err};
use satrs_core::pool::StoreAddr;
use satrs_core::pus::hk::Subservice;
use satrs_core::pus::verification::{
    FailParams, StdVerifSenderError, VerificationReporterWithSender,
};
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
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 serde::{Deserialize, Serialize};
use std::ops::{Deref};
use std::sync::mpsc::{Receiver, Sender};
use std::sync::{Arc, Mutex};
use satrs_core::hk::HkRequest;

pub type CollectionIntervalFactor = u32;

#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum AocsHkIds {
    TestAocsSet = 1,
    TestMgmSet = 2,
}

#[derive(Serialize, Deserialize)]
pub struct AOCSSensorData {
    mgm_data: MGMData,  // Voltage for 3 axis
    css_data: CSSData, // Voltage for 18 sun sensors
    str_data: [f64; 4],  // Quaternion for position of satellite
}

impl AOCSSensorData {
    pub fn new() -> AOCSSensorData {
        let mgm_data = MGMData::default();
        let css_data = CSSData::default();
        let str_data = [0.0; 4];
        AOCSSensorData {
            mgm_data,
            css_data,
            str_data,
        }
    }

    pub fn update_mgm_data(&mut self, mgm_data: &Arc<Mutex<MGMData>>) {
        let data = mgm_data.lock().unwrap();
        self.mgm_data = *data;
    }

    pub fn update_css_data(&mut self, css_data: &Arc<Mutex<CSSData>>) {
        let data = css_data.lock().unwrap();
        self.css_data = *data;
    }

    pub fn write_str_data(&mut self, str_data: [f64; 4]) {
        self.str_data = str_data;
    }

    pub fn get_mgm_data(&mut self) -> MGMData {
        self.mgm_data
    }

    pub fn read_css_data(&mut self) -> CSSData {
        self.css_data
    }

    pub fn read_str_data(&mut self) -> [f64; 4] {
        self.str_data
    }
}

pub struct AOCSHousekeeper {
    sensor_data_pool: Arc<Mutex<AOCSSensorData>>,
    request_rx: Receiver<RequestWithToken>,
    seq_count_provider: SeqCountProviderSyncClonable,
    aocs_tm_store: TmStore,
    aocs_tm_funnel_tx: Sender<StoreAddr>,
    verif_reporter: VerificationReporterWithSender<StdVerifSenderError>,
    periodic_hk_enabled: bool,
    periodic_hk_id: Option<AddressableId>,
}

impl AOCSHousekeeper {
    pub fn new(
        sensor_data_pool: Arc<Mutex<AOCSSensorData>>,
        request_rx: Receiver<RequestWithToken>,
        seq_count_provider: SeqCountProviderSyncClonable,
        aocs_tm_store: TmStore,
        aocs_tm_funnel_tx: Sender<StoreAddr>,
        verif_reporter: VerificationReporterWithSender<StdVerifSenderError>,
    ) -> AOCSHousekeeper {
        AOCSHousekeeper {
            sensor_data_pool,
            request_rx,
            seq_count_provider,
            aocs_tm_store,
            aocs_tm_funnel_tx,
            verif_reporter,
            periodic_hk_enabled: false,
            periodic_hk_id: None,
        }
    }

    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, Some(&time_stamp_buf))
                    .expect("Error sending start success");
                if let Ok(()) = match hk_req {
                    HkRequest::OneShot(id) => self.one_shot_hk(id),
                    HkRequest::Enable(id) => self.enable_hk(id),
                    HkRequest::Disable(id) => self.disable_hk(id),
                    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 periodic_hk(&mut self) {
        if self.periodic_hk_enabled {
            let json_string = self.aocs_data_to_str();
            if let Some(id) = self.periodic_hk_id {
                self.send_hk_packet(id, &json_string);
            }
        }
    }

    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<(), ()> {
        self.periodic_hk_enabled = true;
        self.periodic_hk_id = Some(id);
        return Ok(())
    }

    pub fn disable_hk(&mut self, _id: AddressableId) -> Result<(), ()> {
        self.periodic_hk_enabled = false;
        self.periodic_hk_id = None;
        return Ok(())
    }

    pub fn aocs_data_to_str(&mut self) -> String {
        let pool = self.sensor_data_pool.lock().unwrap();
        serde_json::to_string(pool.deref()).unwrap()
    }

    pub fn send_hk_packet(&mut self, id: AddressableId, 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();
        let mut len = id.write_to_be_bytes(&mut huge_buf).unwrap();
        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");
    }
}