ops-sat-rs/src/tmtc.rs

use crate::pus::PusReceiver;
use satrs::pool::{StoreAddr, StoreError};
use satrs::pus::{EcssTcAndToken, MpscTmAsVecSender};
use satrs::spacepackets::ecss::PusPacket;
use satrs::{
    pus::ReceivesEcssPusTc,
    spacepackets::{ecss::tc::PusTcReader, SpHeader},
    tmtc::ReceivesCcsdsTc,
};
use std::sync::mpsc::{self, SendError, Sender, TryRecvError};
use thiserror::Error;

#[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum MpscStoreAndSendError {
    #[error("Store error: {0}")]
    Store(#[from] StoreError),
    #[error("TC send error: {0}")]
    TcSend(#[from] SendError<EcssTcAndToken>),
    #[error("TMTC send error: {0}")]
    TmTcSend(#[from] SendError<StoreAddr>),
}

// Newtype, can not implement necessary traits on MPSC sender directly because of orphan rules.
#[derive(Clone)]
pub struct PusTcSourceProviderDynamic(pub Sender<Vec<u8>>);

impl ReceivesEcssPusTc for PusTcSourceProviderDynamic {
    type Error = SendError<Vec<u8>>;

    fn pass_pus_tc(&mut self, _: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
        self.0.send(pus_tc.raw_data().to_vec())?;
        Ok(())
    }
}

impl ReceivesCcsdsTc for PusTcSourceProviderDynamic {
    type Error = mpsc::SendError<Vec<u8>>;

    fn pass_ccsds(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
        self.0.send(tc_raw.to_vec())?;
        Ok(())
    }
}

// TC source components where the heap is the backing memory of the received telecommands.
pub struct TcSourceTaskDynamic {
    pub tc_receiver: mpsc::Receiver<Vec<u8>>,
    pus_receiver: PusReceiver<MpscTmAsVecSender>,
}

impl TcSourceTaskDynamic {
    pub fn new(
        tc_receiver: mpsc::Receiver<Vec<u8>>,
        pus_receiver: PusReceiver<MpscTmAsVecSender>,
    ) -> Self {
        Self {
            tc_receiver,
            pus_receiver,
        }
    }

    pub fn periodic_operation(&mut self) {
        self.poll_tc();
    }

    pub fn poll_tc(&mut self) -> bool {
        match self.tc_receiver.try_recv() {
            Ok(tc) => match PusTcReader::new(&tc) {
                Ok((pus_tc, _)) => {
                    self.pus_receiver
                        .handle_tc_packet(
                            satrs::pus::TcInMemory::Vec(tc.clone()),
                            pus_tc.service(),
                            &pus_tc,
                        )
                        .ok();
                    true
                }
                Err(e) => {
                    log::warn!("error creating PUS TC from raw data: {e}");
                    log::warn!("raw data: {:x?}", tc);
                    true
                }
            },
            Err(e) => match e {
                TryRecvError::Empty => false,
                TryRecvError::Disconnected => {
                    log::warn!("tmtc thread: sender disconnected");
                    false
                }
            },
        }
    }
}