firstadd

apps/to/src/to.rs

@@ -0,0 +1,151 @@
+#![no_std]
+extern crate alloc;
+use alloc::vec::Vec;
+use anyhow::Result;
+use connector::Connector;
+use hashbrown::HashMap;
+use log::*;
+use msg::{Instance, Msg, MsgKind, TargetMsg, TlmSetId, ToCmd, ToHk, ToOutData, ToTlmSet};
+use rfe::*;
+
+#[derive(Debug, Clone, Default)]
+pub struct ToData {
+    out_data: ToOutData,
+    hk: ToHk,
+}
+
+pub struct To<'a> {
+    data: ToData,
+    connector: &'a mut dyn Connector,
+    tlm_sets: HashMap<TlmSetId, ToTlmSet>,
+}
+
+impl<'a> To<'a> {
+    pub fn new(connector: &'a mut dyn Connector, tlm_sets: HashMap<TlmSetId, ToTlmSet>) -> Self {
+        Self {
+            connector,
+            data: Default::default(),
+            tlm_sets,
+        }
+    }
+
+    pub fn update_subscriptions(&mut self, rfe: &mut Rfe) {
+        rfe.unsubscribe_all();
+        rfe.subscribe_all(
+            self.tlm_sets
+                .values()
+                .filter(|x| x.enabled)
+                .map(|x| &x.items)
+                .flatten()
+                .map(|x| x.target),
+        );
+    }
+
+    pub fn handle_cmd(&mut self, rfe: &mut Rfe, cmd: &ToCmd) {
+        match cmd {
+            ToCmd::Noop => info!("received Noop"),
+            ToCmd::Reset => {
+                info!("received Reset");
+                self.data = Default::default();
+            }
+            ToCmd::AddTlmSet(to_tlm_set) => {
+                info!("received AddTlmSet");
+                if let Err(e) = self.tlm_sets.try_insert(to_tlm_set.id, to_tlm_set.clone()) {
+                    error!("Could not add tlm set {} {e}", to_tlm_set.id);
+                } else {
+                    info!("TlmSet {} added", to_tlm_set.id);
+                    self.update_subscriptions(rfe);
+                }
+            }
+            ToCmd::RemoveTlmSet(set_id) => {
+                info!("received RemoveTlmSet");
+                if let Some(_set) = self.tlm_sets.remove(set_id) {
+                    info!("removed tlm set {}", set_id);
+                    self.update_subscriptions(rfe);
+                } else {
+                    warn!("Cannot remove tlm set {}, does not exist", set_id);
+                }
+            }
+            ToCmd::DisableTlmSet(set_id) => {
+                info!("received DisableTlmSet");
+                if let Some(set) = self.tlm_sets.get_mut(set_id) {
+                    info!("set {set_id} is now disabled");
+                    set.enabled = false;
+                    self.update_subscriptions(rfe);
+                } else {
+                    warn!("could not disable set {set_id}, does not exist");
+                }
+            }
+            ToCmd::EnablTlmSet(set_id) => {
+                info!("received EnablTlmSet");
+                if let Some(set) = self.tlm_sets.get_mut(set_id) {
+                    info!("set {set_id} is now enabled");
+                    set.enabled = true;
+                    self.update_subscriptions(rfe);
+                } else {
+                    warn!("could not enable set {set_id}, does not exist");
+                }
+            }
+        }
+        info!("got cmd {:?}", cmd);
+    }
+}
+
+impl App for To<'_> {
+    fn init(&mut self, rfe: &mut Rfe) -> Result<()> {
+        rfe.subscribe(TargetMsg::new(rfe.get_instance(), MsgKind::ToCmd));
+        self.update_subscriptions(rfe);
+        return Ok(());
+    }
+
+    fn run(&mut self, rfe: &mut Rfe) {
+        self.data.out_data.counter += 1;
+        let mut msgs = Vec::new();
+        while let Some(msg) = rfe.recv() {
+            let mut is_cmd = false;
+            if let Msg::ToCmd(cmd) = &msg.msg {
+                if msg.instance == rfe.get_instance() {
+                    is_cmd = true;
+                    self.handle_cmd(rfe, cmd);
+                }
+            }
+            if !is_cmd {
+                for tlm_set in self.tlm_sets.values_mut().filter(|x| x.enabled) {
+                    for item in &mut tlm_set.items {
+                        let msg_target = msg.to_target();
+                        if msg_target == item.target
+                            || (msg_target.msg == item.target.msg
+                                && item.target.instance == Instance::All)
+                        {
+                            if item.counter % (item.decimation + 1) == 0 {
+                                msgs.push(msg.clone());
+                            }
+                            item.counter += 1;
+                        }
+                    }
+                }
+            }
+        }
+        if msgs.len() > 0 {
+            self.connector.send(msgs);
+        }
+
+        while let Some(msgs) = self.connector.recv() {
+            for msg in msgs {
+                rfe.post_message(msg);
+            }
+        }
+    }
+
+    fn hk(&mut self, rfe: &mut Rfe) {
+        rfe.send(Msg::ToHk(self.data.hk));
+    }
+
+    fn out_data(&mut self, rfe: &mut Rfe) {
+        rfe.send(Msg::ToOutData(self.data.out_data));
+    }
+
+    fn get_app_rate(&self) -> Rate {
+        Rate::Hz50
+    }
+}