From 2a2a3a3eabd2af2ed733717a0751380794432681 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 22 May 2024 18:48:46 +0200
Subject: [PATCH] PCDU switch set TM handling

---
 satrs-example/src/acs/mgm.rs    |  2 +-
 satrs-example/src/eps/pcdu.rs   | 70 +++++++++++++++++++++++++++++++--
 satrs-example/src/pus/action.rs |  2 +-
 3 files changed, 68 insertions(+), 6 deletions(-)

diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs
index aa85dad..51b4fca 100644
--- a/satrs-example/src/acs/mgm.rs
+++ b/satrs-example/src/acs/mgm.rs
@@ -519,7 +519,7 @@ mod tests {
         pub fn new() -> Self {
             let (request_tx, request_rx) = mpsc::channel();
             let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel();
-            let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::channel();
+            let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5);
             let mode_interface = MpscModeLeafInterface {
                 request_rx,
                 reply_to_pus_tx: reply_tx_to_pus,
diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs
index 6684648..8dce829 100644
--- a/satrs-example/src/eps/pcdu.rs
+++ b/satrs-example/src/eps/pcdu.rs
@@ -5,21 +5,34 @@ use std::{
 };
 
 use derive_new::new;
+use num_enum::{IntoPrimitive, TryFromPrimitive};
 use satrs::{
     hk::{HkRequest, HkRequestVariant},
     mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler},
     power::SwitchRequest,
-    pus::EcssTmSender,
+    pus::{EcssTmSender, PusTmVariant},
     queue::{GenericSendError, GenericTargetedMessagingError},
     request::{GenericMessage, MessageMetadata, UniqueApidTargetId},
+    spacepackets::{
+        ecss::{
+            hk,
+            tm::{PusTmCreator, PusTmSecondaryHeader},
+        },
+        SpHeader,
+    },
 };
 use satrs_example::{config::components::PUS_MODE_SERVICE, DeviceMode, TimestampHelper};
 use satrs_minisim::{
     eps::{PcduReply, PcduRequest, PcduSwitch, SwitchMap, SwitchMapBinaryWrapper},
     SerializableSimMsgPayload, SimReply, SimRequest,
 };
+use serde::{Deserialize, Serialize};
 
-use crate::{acs::mgm::MpscModeLeafInterface, pus::hk::HkReply, requests::CompositeRequest};
+use crate::{
+    acs::mgm::MpscModeLeafInterface,
+    pus::hk::{HkReply, HkReplyVariant},
+    requests::CompositeRequest,
+};
 
 pub trait SerialInterface {
     type Error: core::fmt::Debug;
@@ -40,6 +53,12 @@ pub struct SerialInterfaceToSim {
     pub sim_reply_rx: mpsc::Receiver<SimReply>,
 }
 
+#[derive(Debug, Copy, Clone, PartialEq, Eq, TryFromPrimitive, IntoPrimitive)]
+#[repr(u32)]
+pub enum SetId {
+    SwitcherSet = 0,
+}
+
 impl SerialInterface for SerialInterfaceToSim {
     type Error = ();
 
@@ -162,7 +181,7 @@ pub enum OpCode {
     PollAndRecvReplies = 1,
 }
 
-#[derive(Clone, PartialEq, Eq, Default)]
+#[derive(Clone, PartialEq, Eq, Default, Serialize, Deserialize)]
 pub struct SwitchSet {
     pub valid: bool,
     pub switch_map: SwitchMap,
@@ -187,6 +206,8 @@ pub struct PcduHandler<ComInterface: SerialInterface, TmSender: EcssTmSender> {
     mode_and_submode: ModeAndSubmode,
     #[new(default)]
     stamp_helper: TimestampHelper,
+    #[new(value = "[0; 256]")]
+    tm_buf: [u8; 256],
 }
 
 impl<ComInterface: SerialInterface, TmSender: EcssTmSender> PcduHandler<ComInterface, TmSender> {
@@ -239,7 +260,48 @@ impl<ComInterface: SerialInterface, TmSender: EcssTmSender> PcduHandler<ComInter
 
     pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) {
         match hk_request.variant {
-            HkRequestVariant::OneShot => todo!(),
+            HkRequestVariant::OneShot => {
+                if hk_request.unique_id == SetId::SwitcherSet as u32 {
+                    self.hk_reply_tx
+                        .send(GenericMessage::new(
+                            *requestor_info,
+                            HkReply::new(hk_request.unique_id, HkReplyVariant::Ack),
+                        ))
+                        .expect("failed to send HK reply");
+                    let sec_header = PusTmSecondaryHeader::new(
+                        3,
+                        hk::Subservice::TmHkPacket as u8,
+                        0,
+                        0,
+                        self.stamp_helper.stamp(),
+                    );
+                    // Send TM down as JSON.
+                    let switch_map_snapshot = self
+                        .shared_switch_map
+                        .lock()
+                        .expect("failed to lock switch map")
+                        .clone();
+                    let switch_map_json = serde_json::to_string(&switch_map_snapshot)
+                        .expect("failed to serialize switch map");
+                    if switch_map_json.len() > self.tm_buf.len() {
+                        log::error!("switch map JSON too large for telemetry buffer");
+                        return;
+                    }
+                    self.tm_buf[0..4].copy_from_slice(&self.id.unique_id.to_be_bytes());
+                    self.tm_buf[4..8].copy_from_slice(&(SetId::SwitcherSet as u32).to_be_bytes());
+                    self.tm_buf[8..8 + switch_map_json.len()]
+                        .copy_from_slice(switch_map_json.as_bytes());
+                    let hk_tm = PusTmCreator::new(
+                        SpHeader::new_from_apid(self.id.apid),
+                        sec_header,
+                        &self.tm_buf[0..8 + switch_map_json.len()],
+                        true,
+                    );
+                    self.tm_sender
+                        .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm))
+                        .expect("failed to send HK TM");
+                }
+            }
             HkRequestVariant::EnablePeriodic => todo!(),
             HkRequestVariant::DisablePeriodic => todo!(),
             HkRequestVariant::ModifyCollectionInterval(_) => todo!(),
diff --git a/satrs-example/src/pus/action.rs b/satrs-example/src/pus/action.rs
index 03d362c..238f1c5 100644
--- a/satrs-example/src/pus/action.rs
+++ b/satrs-example/src/pus/action.rs
@@ -341,7 +341,7 @@ mod tests {
             let (tm_funnel_tx, tm_funnel_rx) = mpsc::channel();
             let (pus_action_tx, pus_action_rx) = mpsc::channel();
             let (action_reply_tx, action_reply_rx) = mpsc::channel();
-            let (action_req_tx, action_req_rx) = mpsc::channel();
+            let (action_req_tx, action_req_rx) = mpsc::sync_channel(10);
             let verif_reporter = TestVerificationReporter::new(owner_id);
             let mut generic_req_router = GenericRequestRouter::default();
             generic_req_router