initial integration with telem viz

Cargo.toml

@@ -4,10 +4,23 @@ members = ["common", "ground", "flight"]
 
 [workspace.dependencies]
 anyhow = "1.0.100"
-fern = { version = "0.7.1", features = ["colored"] }
+api = { git = "https://gitea.sergeysav.com/sergeysav/telemetry_visualization.git", rev = "458c94c2ad362b1a6c31c2b1ee606a9f40605e06" }
-log = { version = "0.4.28" }
 chrono = { version = "0.4.42", features = ["serde"] }
+crc = "3.4.0"
 ctrlc = "3.5.0"
-serde = { version = "1.0.228", features = ["derive"], default-features = false }
+derive_more = "2.1.1"
+embedded-hal = "1.0.0"
+embedded-hal-bus = "0.3.0"
+embedded-hal-mock = "0.11.1"
+fern = { version = "0.7.1", features = ["colored"] }
+futures = "0.3.31"
+itertools = "0.14.0"
+log = { version = "0.4.29" }
+nautilus_common = { path = "./common" }
 postcard = { version = "1.1.3", default-features = false, features = ["alloc"] }
+rpi-pal = "0.22.2"
+serde = { version = "1.0.228", features = ["derive"], default-features = false }
 thiserror = "2.0.17"
+tokio = { version = "1.48.0" }
+tokio-util = "0.7.17"
+

common/Cargo.toml

@@ -5,10 +5,16 @@ edition = "2024"
 
 [dependencies]
 anyhow = { workspace = true }
-fern = { workspace = true }
-log = { workspace = true }
 chrono = { workspace = true }
 ctrlc = { workspace = true }
-serde = { workspace = true }
+derive_more = {workspace = true, features = ["display"]}
+fern = { workspace = true }
+log = { workspace = true }
 postcard = { workspace = true }
+serde = { workspace = true }
 thiserror = { workspace = true }
+tokio = { workspace = true, optional = true, features = ["net"] }
+tokio-util = {workspace = true, optional = true}
+
+[features]
+async = [ "dep:tokio", "dep:tokio-util" ]

common/src/command/mod.rs

@@ -50,6 +50,12 @@ pub struct CommandHeader<'a> {
     pub data: &'a [u8],
 }
 
+#[derive(Clone, Debug, Serialize, Deserialize)]
+pub struct OwnedCommandHeader {
+    pub name: String,
+    pub data: Vec<u8>,
+}
+
 pub trait Command: Serialize + DeserializeOwned {}
 
 impl Command for () {}

common/src/lib.rs

@@ -1,10 +1,9 @@
 #![warn(clippy::all, clippy::pedantic)]
 use log::info;
-use std::sync::Arc;
-use std::sync::atomic::{AtomicBool, Ordering};
 
 pub mod command;
 pub mod logger;
+pub mod on_drop;
 pub mod telemetry;
 pub mod udp;
 
@@ -12,10 +11,25 @@ pub mod udp;
 ///
 /// # Errors
 /// If a system error occurred while trying to set the ctrl-c handler
-pub fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> anyhow::Result<()> {
+pub fn add_ctrlc_handler_arc(
+    flag: std::sync::Arc<std::sync::atomic::AtomicBool>,
+) -> anyhow::Result<()> {
     ctrlc::set_handler(move || {
         info!("Shutdown Requested");
-        flag.store(false, Ordering::Relaxed);
+        flag.store(false, std::sync::atomic::Ordering::Relaxed);
+    })?;
+    Ok(())
+}
+
+/// Add a ctrl-c handler which will set an atomic flag to `false` when ctrl-c is detected
+///
+/// # Errors
+/// If a system error occurred while trying to set the ctrl-c handler
+#[cfg(feature = "async")]
+pub fn add_ctrlc_handler_cancel(cancel: tokio_util::sync::CancellationToken) -> anyhow::Result<()> {
+    ctrlc::set_handler(move || {
+        info!("Shutdown Requested");
+        cancel.cancel();
     })?;
     Ok(())
 }

common/src/logger.rs

@@ -1,6 +1,6 @@
 use anyhow::Result;
 use fern::colors::{Color, ColoredLevelConfig};
-use log::debug;
+use log::{LevelFilter, debug};
 use std::fs::create_dir_all;
 use std::str::FromStr;
 use std::{env, thread};
@@ -46,6 +46,9 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> {
         .chain(
             fern::Dispatch::new()
                 .level(log_level)
+                .level_for("tungstenite", LevelFilter::Warn)
+                .level_for("tokio_tungstenite", LevelFilter::Warn)
+                .level_for("api", LevelFilter::Info)
                 .chain(std::io::stdout()),
         )
         .chain(fern::log_file(log_file.clone())?)

common/src/telemetry/mod.rs

@@ -1,5 +1,6 @@
 use chrono::serde::ts_nanoseconds;
 use chrono::{DateTime, Utc};
+use derive_more::Display;
 use serde::{Deserialize, Serialize};
 
 #[derive(Clone, Debug, Serialize, Deserialize)]
@@ -9,7 +10,7 @@ pub struct Telemetry {
     pub message: TelemetryMessage,
 }
 
-#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
+#[derive(Copy, Clone, Debug, Display, Serialize, Deserialize)]
 pub enum SwitchBank {
     A,
     B,

common/src/udp.rs

@@ -1,7 +1,6 @@
 use crate::command::{Command, CommandHeader};
 use crate::udp::UdpRecvPostcardError::ExtraData;
 use crate::udp::UdpSendPostcardError::LengthMismatch;
-use log::error;
 use serde::{Deserialize, Serialize};
 use std::io::ErrorKind;
 use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
@@ -30,7 +29,7 @@ pub enum UdpSendPostcardError {
 }
 
 pub trait UdpSocketExt {
-    /// Receive a CBOR encoded message from this UDP Socket
+    /// Receive a Postcard encoded message from this UDP Socket
     ///
     /// # Errors
     /// An error that could have occurred while trying to receive this message.
@@ -63,26 +62,51 @@ pub trait UdpSocketExt {
     ) -> Result<(), UdpSendPostcardError>;
 }
 
+fn recv_postcard_inner<'de, T: Deserialize<'de>>(
+    result: std::io::Result<(usize, SocketAddr)>,
+    buffer: &'de mut [u8],
+) -> Result<(T, SocketAddr), UdpRecvPostcardError> {
+    match result {
+        Ok((size, addr)) => match postcard::take_from_bytes::<T>(&buffer[..size]) {
+            Ok((res, rem)) => {
+                if !rem.is_empty() {
+                    return Err(ExtraData { amount: rem.len() });
+                }
+                Ok((res, addr))
+            }
+            Err(err) => Err(err.into()),
+        },
+        Err(err) => match err.kind() {
+            ErrorKind::WouldBlock | ErrorKind::TimedOut => Err(UdpRecvPostcardError::NoData),
+            _ => Err(err.into()),
+        },
+    }
+}
+
+fn send_inner(
+    send_result: Result<usize, std::io::Error>,
+    expected_size: usize,
+) -> Result<(), UdpSendPostcardError> {
+    match send_result {
+        Ok(size_sent) => {
+            if expected_size != size_sent {
+                return Err(LengthMismatch {
+                    expected: expected_size,
+                    actual: size_sent,
+                });
+            }
+            Ok(())
+        }
+        Err(e) => Err(e.into()),
+    }
+}
+
 impl UdpSocketExt for UdpSocket {
     fn recv_postcard<'de, T: Deserialize<'de>>(
         &self,
         buffer: &'de mut [u8],
     ) -> Result<(T, SocketAddr), UdpRecvPostcardError> {
-        match self.recv_from(buffer) {
+        recv_postcard_inner(self.recv_from(buffer), buffer)
-            Ok((size, addr)) => match postcard::take_from_bytes::<T>(&buffer[..size]) {
-                Ok((res, rem)) => {
-                    if !rem.is_empty() {
-                        return Err(ExtraData { amount: rem.len() });
-                    }
-                    Ok((res, addr))
-                }
-                Err(err) => Err(err.into()),
-            },
-            Err(err) => match err.kind() {
-                ErrorKind::WouldBlock | ErrorKind::TimedOut => Err(UdpRecvPostcardError::NoData),
-                _ => Err(err.into()),
-            },
-        }
     }
 
     fn send_postcard<T: Serialize + ?Sized, A: ToSocketAddrs>(
@@ -91,24 +115,9 @@ impl UdpSocketExt for UdpSocket {
         buffer: &mut [u8],
         addr: A,
     ) -> Result<(), UdpSendPostcardError> {
-        match postcard::to_slice(data, buffer) {
+        let result = postcard::to_slice(data, buffer)?;
-            Ok(result) => {
+        let size_encoded = result.len();
-                let size_encoded = result.len();
+        send_inner(self.send_to(result, addr), size_encoded)
-                match self.send_to(result, addr) {
-                    Ok(size_sent) => {
-                        if size_encoded != size_sent {
-                            return Err(LengthMismatch {
-                                expected: size_encoded,
-                                actual: size_sent,
-                            });
-                        }
-                        Ok(())
-                    }
-                    Err(e) => Err(e.into()),
-                }
-            }
-            Err(e) => Err(e.into()),
-        }
     }
 
     fn send_command<T: Command, A: ToSocketAddrs>(
@@ -120,21 +129,97 @@ impl UdpSocketExt for UdpSocket {
         let mut inner_buffer = [0u8; 512];
         let inner_buffer = postcard::to_slice(data, &mut inner_buffer)?;
         let mut buffer = [0u8; 512];
-        let buffer = postcard::to_slice(
+        self.send_postcard(
             &CommandHeader {
                 name,
                 data: inner_buffer,
             },
             &mut buffer,
-        )?;
+            addr,
-        let size_encoded = buffer.len();
+        )
-        let size_sent = self.send_to(buffer, addr)?;
+    }
-        if size_encoded != size_sent {
+}
-            return Err(LengthMismatch {
+
-                expected: size_encoded,
+#[cfg(feature = "async")]
-                actual: size_sent,
+pub mod tokio {
-            });
+    use super::{
-        }
+        Command, CommandHeader, Deserialize, Serialize, SocketAddr, UdpRecvPostcardError,
-        Ok(())
+        UdpSendPostcardError, recv_postcard_inner, send_inner,
+    };
+    use ::tokio::net::ToSocketAddrs;
+    use ::tokio::net::UdpSocket;
+
+    pub trait AsyncUdpSocketExt {
+        /// Receive a Postcard encoded message from this UDP Socket
+        ///
+        /// # Errors
+        /// An error that could have occurred while trying to receive this message.
+        /// If no data was received a `UdpRecvCborError::NoData` error would be returned.
+        fn recv_postcard<'de, T: Deserialize<'de>>(
+            &self,
+            buffer: &'de mut [u8],
+        ) -> impl Future<Output = Result<(T, SocketAddr), UdpRecvPostcardError>>;
+
+        /// Send a CBOR encoded message to an address using this socket
+        ///
+        /// # Errors
+        /// An error that could have occurred while trying to send this message
+        fn send_postcard<T: Serialize + ?Sized, A: ToSocketAddrs>(
+            &self,
+            data: &T,
+            buffer: &mut [u8],
+            addr: A,
+        ) -> impl Future<Output = Result<(), UdpSendPostcardError>>;
+
+        /// Send a command message to an address using this socket
+        ///
+        /// # Errors
+        /// An error that could have occurred while trying to send this message
+        fn send_command<T: Command, A: ToSocketAddrs>(
+            &self,
+            name: &str,
+            data: &T,
+            addr: A,
+        ) -> impl Future<Output = Result<(), UdpSendPostcardError>>;
+    }
+
+    impl AsyncUdpSocketExt for UdpSocket {
+        async fn recv_postcard<'de, T: Deserialize<'de>>(
+            &self,
+            buffer: &'de mut [u8],
+        ) -> Result<(T, SocketAddr), UdpRecvPostcardError> {
+            recv_postcard_inner(self.recv_from(buffer).await, buffer)
+        }
+
+        async fn send_postcard<T: Serialize + ?Sized, A: ToSocketAddrs>(
+            &self,
+            data: &T,
+            buffer: &mut [u8],
+            addr: A,
+        ) -> Result<(), UdpSendPostcardError> {
+            let result = postcard::to_slice(data, buffer)?;
+            let size_encoded = result.len();
+            send_inner(self.send_to(result, addr).await, size_encoded)
+        }
+
+        async fn send_command<T: Command, A: ToSocketAddrs>(
+            &self,
+            name: &str,
+            data: &T,
+            addr: A,
+        ) -> Result<(), UdpSendPostcardError> {
+            let mut inner_buffer = [0u8; 512];
+            let inner_buffer = postcard::to_slice(data, &mut inner_buffer)?;
+            let mut buffer = [0u8; 512];
+            self.send_postcard(
+                &CommandHeader {
+                    name,
+                    data: inner_buffer,
+                },
+                &mut buffer,
+                addr,
+            )
+            .await
+        }
     }
 }

flight/Cargo.toml

@@ -5,19 +5,18 @@ edition = "2024"
 
 [dependencies]
 anyhow = { workspace = true }
-log = { workspace = true, features = ["max_level_trace", "release_max_level_debug"] }
 chrono = { workspace = true }
-embedded-hal = "1.0.0"
+crc = {workspace = true}
-embedded-hal-bus = { version = "0.3.0", features = ["std"] }
+embedded-hal = {workspace = true}
-embedded-hal-mock = { version = "0.11.1", optional = true }
+embedded-hal-bus = { workspace = true, features = ["std"] }
-rpi-pal = { version = "0.22.2", features = ["hal"], optional = true }
+embedded-hal-mock = { workspace = true, optional = true }
-hex = "0.4.3"
+log = { workspace = true, features = ["max_level_trace", "release_max_level_debug"] }
-crc = "3.3.0"
+nautilus_common = { workspace = true }
-nautilus_common = { path = "../common" }
 postcard = { workspace = true }
+rpi-pal = { workspace = true, features = ["hal"], optional = true }
 
 [dev-dependencies]
-embedded-hal-mock = { version = "0.11.1" }
+embedded-hal-mock = { workspace = true }
 
 [features]
 raspi = ["dep:rpi-pal"]

flight/src/lib.rs

@@ -10,12 +10,12 @@ use crate::state_vector::StateVector;
 use anyhow::Result;
 use embedded_hal::pwm::SetDutyCycle;
 use log::info;
-use nautilus_common::add_ctrlc_handler;
 use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
 use std::sync::Arc;
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread::sleep;
 use std::time::Duration;
+use nautilus_common::add_ctrlc_handler_arc;
 
 mod hardware;
 
@@ -35,7 +35,7 @@ pub fn run() -> Result<()> {
     );
 
     let running = Arc::new(AtomicBool::new(true));
-    add_ctrlc_handler(running.clone())?;
+    add_ctrlc_handler_arc(running.clone())?;
 
     let state_vector = StateVector::new();
 
@@ -92,7 +92,7 @@ pub fn run() -> Result<()> {
                     });
                 });
             },
-            1,
+            10,
         )?;
 
         info!("Starting Main Loop");
@@ -119,7 +119,6 @@ pub fn run() -> Result<()> {
 
 mod comms;
 mod data;
-mod on_drop;
 mod rcs;
 mod scheduler;
 mod state_vector;

flight/src/scheduler/mod.rs

@@ -1,6 +1,6 @@
-use crate::on_drop::on_drop;
 use anyhow::Result;
 use log::trace;
+use nautilus_common::on_drop::on_drop;
 use std::any::type_name;
 use std::fmt::Debug;
 use std::ops::Deref;

ground/Cargo.toml

@@ -5,9 +5,12 @@ edition = "2024"
 
 [dependencies]
 anyhow = { workspace = true }
-nautilus_common = { path = "../common" }
+api = { workspace = true }
-log = { workspace = true }
 chrono = { workspace = true }
+futures = {workspace = true}
+itertools = { workspace = true }
+log = { workspace = true }
+nautilus_common = { workspace = true, features = ["async"] }
 postcard = { workspace = true }
-hex = "0.4.3"
+tokio = { workspace = true, features = ["rt-multi-thread", "rt", "macros", "net"]}
-serde = { workspace = true }
+tokio-util = { workspace = true }

ground/src/command.rs

@@ -0,0 +1,121 @@
+use api::client::Client;
+use api::client::command::CommandRegistry;
+use api::macros::IntoCommandDefinition;
+use chrono::TimeDelta;
+use itertools::Itertools;
+use log::{error, trace};
+use nautilus_common::command::{Command, OwnedCommandHeader, SetPin, ValidPriorityCommand};
+use nautilus_common::udp::tokio::AsyncUdpSocketExt;
+use std::fmt::Debug;
+use std::net::SocketAddr;
+use std::sync::Arc;
+use tokio::net::UdpSocket;
+use tokio::select;
+use tokio::sync::RwLock;
+use tokio::sync::mpsc::{Sender, channel};
+use tokio_util::sync::CancellationToken;
+
+pub struct CommandHandler<'a> {
+    cmd: CommandRegistry,
+    flight_addr: &'a RwLock<SocketAddr>,
+    udp: &'a UdpSocket,
+    cancel: CancellationToken,
+}
+
+#[derive(IntoCommandDefinition)]
+struct SetPinCommand {
+    state: bool,
+}
+
+impl<'a> CommandHandler<'a> {
+    pub fn new(
+        client: Arc<Client>,
+        flight_addr: &'a RwLock<SocketAddr>,
+        udp: &'a UdpSocket,
+        cancel: CancellationToken,
+    ) -> Self {
+        Self {
+            cmd: CommandRegistry::new(client),
+            flight_addr,
+            udp,
+            cancel,
+        }
+    }
+
+    pub fn run(self) -> anyhow::Result<()> {
+        let runtime = tokio::runtime::Builder::new_current_thread()
+            .enable_all()
+            .build()?;
+
+        runtime.block_on(async move {
+            let (outgoing_commands_tx, mut outgoing_commands_rx) = channel::<OwnedCommandHeader>(16);
+
+            let commands = ["a", "b"].iter().cartesian_product(0..16)
+                .map(|(bank, i)| {
+                    let command_name = format!("/mcp23017{bank}/set");
+                    let outgoing_commands_tx = outgoing_commands_tx.clone();
+                    self.cmd.register_handler(
+                        format!("switch.{bank}.{i}.set"),
+                        move |header, cmd: SetPinCommand| -> anyhow::Result<_> {
+                            trace!("Setting Switch {bank} {i}");
+
+                            outgoing_commands_tx.try_send_command(
+                                &command_name,
+                                &ValidPriorityCommand {
+                                    inner: SetPin {
+                                        pin: i,
+                                        value: cmd.state,
+                                    },
+                                    valid_until: header.timestamp + TimeDelta::seconds(5),
+                                    priority: 0,
+                                }
+                            )?;
+
+                            Ok("Command Executed Successfully".to_string())
+                        }
+                    )
+                })
+                .collect::<Vec<_>>();
+
+            let mut buffer = [0u8; 512];
+            while !self.cancel.is_cancelled() {
+                select! {
+                    () = self.cancel.cancelled() => { break; }
+                    outgoing = outgoing_commands_rx.recv() => {
+                        match outgoing {
+                            None => break,
+                            Some(outgoing) => {
+                                match self.udp.send_postcard(&outgoing, &mut buffer, *self.flight_addr.read().await).await {
+                                    Ok(()) => {},
+                                    Err(err) => error!("Failed to Send Outgoing {err}"),
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            // Explicit Drops
+            drop(commands);
+            drop(self);
+        });
+
+        Ok(())
+    }
+}
+
+trait SenderExt {
+    fn try_send_command<T: Command + Debug>(&self, name: &str, data: &T) -> anyhow::Result<()>;
+}
+
+impl SenderExt for Sender<OwnedCommandHeader> {
+    fn try_send_command<T: Command + Debug>(&self, name: &str, data: &T) -> anyhow::Result<()> {
+        trace!("{data:?}");
+        let inner_buffer = postcard::to_allocvec(data)?;
+        self.try_send(OwnedCommandHeader {
+            name: name.to_string(),
+            data: inner_buffer,
+        })?;
+        Ok(())
+    }
+}

ground/src/lib.rs

@@ -1,15 +1,24 @@
 #![warn(clippy::all, clippy::pedantic)]
-use anyhow::Result;
+
-use chrono::{TimeDelta, Utc};
+use nautilus_common::udp::tokio::AsyncUdpSocketExt;
-use log::{error, info};
+use std::net::{IpAddr, Ipv4Addr, SocketAddr};
-use nautilus_common::add_ctrlc_handler;
+mod command;
-use nautilus_common::command::{SetPin, ValidPriorityCommand};
+mod telemetry;
-use nautilus_common::telemetry::Telemetry;
+
-use nautilus_common::udp::{UdpRecvPostcardError, UdpSocketExt};
+use crate::command::CommandHandler;
-use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket};
+use crate::telemetry::TelemetryHandler;
+use anyhow::{Result, anyhow, bail};
+use api::client::Client;
+use futures::executor::block_on;
+use futures::future::{Either, select};
+use log::{info, warn};
+use nautilus_common::add_ctrlc_handler_cancel;
 use std::sync::Arc;
-use std::sync::atomic::{AtomicBool, Ordering};
+use std::thread::{Builder, scope};
-use std::time::Duration;
+use tokio::net::UdpSocket;
+use tokio::pin;
+use tokio::sync::RwLock;
+use tokio_util::sync::CancellationToken;
 
 /// Run the Ground Software
 ///
@@ -21,86 +30,78 @@ pub fn run() -> Result<()> {
         env!("CARGO_PKG_VERSION")
     );
 
-    let running = Arc::new(AtomicBool::new(true));
+    let udp_shutdown = CancellationToken::new();
+    let cancel = udp_shutdown.child_token();
+    add_ctrlc_handler_cancel(cancel.clone())?;
 
-    add_ctrlc_handler(running.clone())?;
+    let (udp_thread, udp) = {
+        let udp_shutdown = udp_shutdown.clone();
+        let (udp_tx, udp) = tokio::sync::oneshot::channel();
 
-    let mut flight_addr = None;
+        let udp_thread = Builder::new()
-    let bind_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), 14000);
+            .name("flight-udp-connection-handler".to_string())
-    let udp = UdpSocket::bind(bind_addr)?;
+            .spawn(move || {
-    udp.set_read_timeout(Some(Duration::from_millis(100)))?;
+                let runtime = tokio::runtime::Builder::new_current_thread()
+                    .enable_all()
+                    .build()?;
 
-    let mut buffer = [0u8; 512];
+                runtime.block_on(async {
+                    let udp = UdpSocket::bind("0.0.0.0:14000").await?;
+                    udp_tx
+                        .send(udp)
+                        .map_err(|_| anyhow!("Couldn't complete UDP establish"))?;
+                    Ok(()) as Result<()>
+                })?;
 
-    let mut do_once = true;
+                runtime.block_on(udp_shutdown.cancelled());
 
-    while running.load(Ordering::Relaxed) {
+                Ok(()) as Result<()>
-        match udp.recv_postcard::<Telemetry>(&mut buffer) {
+            })?;
-            Ok((tlm, addr)) => {
-                flight_addr = Some(addr);
-                info!("{tlm:?}");
 
-                if do_once {
+        let f1 = cancel.cancelled();
-                    do_once = false;
+        pin!(f1);
-                    udp.send_command(
+        let udp = block_on(async {
-                        "/mcp23017a/set",
+            let f2 = udp;
-                        &ValidPriorityCommand {
+            select(f1, f2).await
-                            inner: SetPin {
+        });
-                                pin: 0,
+        let udp = match udp {
-                                value: true,
+            Either::Left(_) => bail!("Cancelled Before UDP established"),
-                            },
+            Either::Right(x) => x.0?,
-                            valid_until: Utc::now() + TimeDelta::seconds(5),
+        };
-                            priority: 0,
+        (udp_thread, udp)
-                        },
+    };
-                        addr,
+
-                    )?;
+    let flight_addr = RwLock::new(SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), 0));
-                }
+    let lock = flight_addr.blocking_write();
-            }
+
-            Err(UdpRecvPostcardError::NoData) => {
+    scope(|scope| {
-                // NoOp
+        let client = Arc::new(Client::connect("ws://localhost:8080/backend")?);
-            }
+
-            Err(err) => {
+        let tlm = TelemetryHandler::new(client.clone(), &flight_addr, lock, &udp, cancel.clone());
-                error!("Rx error: {err}");
+        let cmd = CommandHandler::new(client, &flight_addr, &udp, cancel.clone());
-            }
+
-        }
+        let tlm_thread = Builder::new()
+            .name("telemetry-handler".to_string())
+            .spawn_scoped(scope, move || tlm.run())?; // move to take ownership (drop when exited)
+
+        let cmd_thread = Builder::new()
+            .name("command-handler".to_string())
+            .spawn_scoped(scope, move || cmd.run())?;
+
+        // Force the thread panics into anyhow errors
+        tlm_thread.join().map_err(|e| anyhow!("{e:?}"))??;
+        cmd_thread.join().map_err(|e| anyhow!("{e:?}"))??;
+        Ok(()) as Result<()>
+    })?;
+
+    info!("Sending Shutdown Command");
+
+    if let Ok(flight_addr) = flight_addr.try_read() {
+        block_on(async { udp.send_command("/shutdown", &(), *flight_addr).await })?;
     }
 
-    if let Some(flight_addr) = flight_addr {
+    udp_shutdown.cancel();
-        udp.send_command("/shutdown", &(), flight_addr)?;
+    udp_thread.join().map_err(|e| anyhow!("{e:?}"))??;
-
-        // let cmd_data = CommandData::Shutdown;
-        // udp.send_postcard(&cmd_data, &mut buffer, flight_addr)?;
-
-        // let cmd_data = SetPin {
-        //     pin: 4,
-        //     value: true,
-        //     valid_until: Utc::now(),
-        //     priority: 120,
-        // };
-        // let cmd_data = postcard::to_allocvec(&cmd_data)?;
-        //
-        // let cmd = CommandHeader {
-        //     name: "/shutdown",
-        //     data: &cmd_data,
-        // };
-        //
-        // let encoded = postcard::to_allocvec(&cmd)?;
-        // println!("{}", hex::encode(&encoded));
-        //
-        // let decoded = postcard::from_bytes::<CommandHeader>(&encoded)?;
-        // println!("{decoded:?}");
-        //
-        // let (decoded, remainder) = postcard::take_from_bytes::<SetPin>(decoded.data)?;
-        // ensure!(remainder.is_empty(),  "Not all command bytes consumed");
-        // println!("{decoded:?}");
-
-        // let mut another_buffer = Cursor::new([0u8; 512]);
-        // // ciborium::into_writer(&cmd, &mut another_buffer)?;
-        // let _ = Serializer::writer(&cmd, &mut another_buffer)?;
-        // let size_encoded = usize::try_from(another_buffer.position())?;
-
-        // let _ = test(&another_buffer.get_ref()[0..size_encoded])?;
-    }
 
     Ok(())
 }

ground/src/telemetry.rs

@@ -0,0 +1,142 @@
+use api::client::Client;
+use api::client::telemetry::{TelemetryHandle, TelemetryRegistry};
+use futures::TryFutureExt;
+use futures::future::join_all;
+use log::{error, trace};
+use nautilus_common::on_drop::on_drop;
+use nautilus_common::telemetry::{SwitchBank, Telemetry, TelemetryMessage};
+use nautilus_common::udp::UdpRecvPostcardError;
+use nautilus_common::udp::tokio::AsyncUdpSocketExt;
+use std::net::SocketAddr;
+use std::sync::Arc;
+use tokio::net::UdpSocket;
+use tokio::sync::{RwLock, RwLockWriteGuard};
+use tokio::task::yield_now;
+use tokio::{join, pin, select};
+use tokio_util::sync::CancellationToken;
+
+pub struct TelemetryHandler<'a> {
+    tlm: TelemetryRegistry,
+    flight_addr: &'a RwLock<SocketAddr>,
+    lock: Option<RwLockWriteGuard<'a, SocketAddr>>,
+    udp: &'a UdpSocket,
+    cancel: CancellationToken,
+}
+
+impl<'a> TelemetryHandler<'a> {
+    pub fn new(
+        client: Arc<Client>,
+        flight_addr: &'a RwLock<SocketAddr>,
+        lock: RwLockWriteGuard<'a, SocketAddr>,
+        udp: &'a UdpSocket,
+        cancel: CancellationToken,
+    ) -> Self {
+        Self {
+            tlm: TelemetryRegistry::new(client),
+            flight_addr,
+            lock: Some(lock),
+            udp,
+            cancel,
+        }
+    }
+
+    /// Run this telemetry handler.
+    /// Note: this method is expected to block so should run in its own thread
+    pub fn run(mut self) -> anyhow::Result<()> {
+        let cancel = self.cancel.clone();
+        // Trigger a shutdown if we exit the telemetry process for some reason (including panic)
+        let _shutdown_when_closed = on_drop(move || cancel.cancel());
+
+        let runtime = tokio::runtime::Builder::new_current_thread()
+            .enable_all()
+            .build()?;
+
+        runtime.block_on(async {
+            let mut context = TelemetryContext::new(&self).await;
+
+            let mut buffer = [0u8; 512];
+
+            while !self.cancel.is_cancelled() {
+                select! {
+                    () = self.cancel.cancelled() => {},
+                    incoming = self.udp.recv_postcard::<Telemetry>(&mut buffer) => {
+                        match incoming {
+                            Ok((tlm, addr)) => {
+                                trace!("{tlm:?}");
+
+                                let flight_addr_update = async {
+                                    // The first time around we will use the lock given to us
+                                    // Other times we will grab a new lock
+                                    // self.re
+                                    let mut lock = match self.lock.take() {
+                                        None => self.flight_addr.write().await,
+                                        Some(lock) => lock,
+                                    };
+                                    // Update the value in the lock
+                                    *lock = addr;
+                                };
+                                pin!(flight_addr_update);
+
+                                // We can do these two operations concurrently
+                                join!(
+                                    flight_addr_update,
+                                    context.step(tlm),
+                                );
+                            },
+                            Err(UdpRecvPostcardError::NoData) => {
+                                // This shouldn't be possible when using a tokio socket I don't think
+                                // But let's just yield our time anyways
+                                yield_now().await;
+                            },
+                            Err(err) => {
+                                error!("Rx error: {err}");
+                            },
+                        }
+                    }
+                }
+            }
+
+            // Explicit Drop
+            drop(self);
+
+            Ok(())
+        })
+    }
+}
+
+struct TelemetryContext {
+    bank_a: Vec<TelemetryHandle<bool>>,
+    bank_b: Vec<TelemetryHandle<bool>>,
+}
+
+impl TelemetryContext {
+    async fn new(tlm: &TelemetryHandler<'_>) -> Self {
+        let bank_a =
+            join_all((0..16).map(|i| tlm.tlm.register::<bool>(format!("switch.bank_a.{i}")))).await;
+        let bank_b =
+            join_all((0..16).map(|i| tlm.tlm.register::<bool>(format!("switch.bank_b.{i}")))).await;
+
+        Self { bank_a, bank_b }
+    }
+
+    async fn step(&mut self, tlm: Telemetry) {
+        match tlm.message {
+            TelemetryMessage::SwitchState { bank, switches } => {
+                let bank_handles = match bank {
+                    SwitchBank::A => &self.bank_a,
+                    SwitchBank::B => &self.bank_b,
+                };
+                assert!(bank_handles.len() >= switches.iter().len());
+                join_all(switches.into_iter().enumerate().map(|(i, state)| {
+                    bank_handles[i]
+                        .publish(state, tlm.timestamp)
+                        .unwrap_or_else(move |err| {
+                            // We don't need to bubble this error up, just report it
+                            error!("Failed to publish telemetry for switch {bank} {i}: {err}");
+                        })
+                }))
+                .await;
+            }
+        }
+    }
+}