sergeysav/ProjectNautilus
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial integration with telem viz

Browse Source
This commit is contained in:
Sergey Savelyev
2026-01-02 15:36:50 -05:00
parent 59b5679dda
commit 275cb07c4c
16 changed files with 1408 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

889
Cargo.lock generated
View File

File diff suppressed because it is too large Load Diff

19
Cargo.toml
View File

@@ -4,10 +4,23 @@ members = ["common", "ground", "flight"]
[workspace.dependencies]
anyhow = "1.0.100"
fern = { version = "0.7.1", features = ["colored"] }
log = { version = "0.4.28" }
api = { git = "https://gitea.sergeysav.com/sergeysav/telemetry_visualization.git", rev = "458c94c2ad362b1a6c31c2b1ee606a9f40605e06" }
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"

12
common/Cargo.toml
View File

@@ -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" ]

6
common/src/command/mod.rs
View File

@@ -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 () {}

22
common/src/lib.rs
View File

@@ -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(())
}

5
common/src/logger.rs
View File

@@ -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())?)

0
flight/src/on_drop.rs → common/src/on_drop.rs
View File

3
common/src/telemetry/mod.rs
View File

@@ -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,

155
common/src/udp.rs
View File

@@ -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,12 +62,11 @@ pub trait UdpSocketExt {
) -> Result<(), UdpSendPostcardError>;
}
impl UdpSocketExt for UdpSocket {
fn recv_postcard<'de, T: Deserialize<'de>>(
&self,
fn recv_postcard_inner<'de, T: Deserialize<'de>>(
result: std::io::Result<(usize, SocketAddr)>,
buffer: &'de mut [u8],
) -> Result<(T, SocketAddr), UdpRecvPostcardError> {
match self.recv_from(buffer) {
) -> Result<(T, SocketAddr), UdpRecvPostcardError> {
match result {
Ok((size, addr)) => match postcard::take_from_bytes::<T>(&buffer[..size]) {
Ok((res, rem)) => {
if !rem.is_empty() {
@@ -83,6 +81,32 @@ impl UdpSocketExt for UdpSocket {
_ => 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> {
recv_postcard_inner(self.recv_from(buffer), buffer)
}
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) {
Ok(result) => {
let result = postcard::to_slice(data, buffer)?;
let size_encoded = result.len();
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()),
}
send_inner(self.send_to(result, addr), size_encoded)
}
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,
)?;
let size_encoded = buffer.len();
let size_sent = self.send_to(buffer, addr)?;
if size_encoded != size_sent {
return Err(LengthMismatch {
expected: size_encoded,
actual: size_sent,
});
}
Ok(())
addr,
)
}
}
#[cfg(feature = "async")]
pub mod tokio {
use super::{
Command, CommandHeader, Deserialize, Serialize, SocketAddr, UdpRecvPostcardError,
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
}
}
}

17
flight/Cargo.toml
View File

@@ -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"
embedded-hal-bus = { version = "0.3.0", features = ["std"] }
embedded-hal-mock = { version = "0.11.1", optional = true }
rpi-pal = { version = "0.22.2", features = ["hal"], optional = true }
hex = "0.4.3"
crc = "3.3.0"
nautilus_common = { path = "../common" }
crc = {workspace = true}
embedded-hal = {workspace = true}
embedded-hal-bus = { workspace = true, features = ["std"] }
embedded-hal-mock = { workspace = true, optional = true }
log = { workspace = true, features = ["max_level_trace", "release_max_level_debug"] }
nautilus_common = { workspace = true }
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"]

7
flight/src/lib.rs
View File

@@ -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;

2
flight/src/scheduler/mod.rs
View File

@@ -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;

11
ground/Cargo.toml
View File

@@ -5,9 +5,12 @@ edition = "2024"
[dependencies]
anyhow = { workspace = true }
nautilus_common = { path = "../common" }
log = { workspace = true }
api = { 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"
serde = { workspace = true }
tokio = { workspace = true, features = ["rt-multi-thread", "rt", "macros", "net"]}
tokio-util = { workspace = true }

121
ground/src/command.rs Normal file
View File

@@ -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(())
}
}

165
ground/src/lib.rs
View File

@@ -1,15 +1,24 @@
#![warn(clippy::all, clippy::pedantic)]
use anyhow::Result;
use chrono::{TimeDelta, Utc};
use log::{error, info};
use nautilus_common::add_ctrlc_handler;
use nautilus_common::command::{SetPin, ValidPriorityCommand};
use nautilus_common::telemetry::Telemetry;
use nautilus_common::udp::{UdpRecvPostcardError, UdpSocketExt};
use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket};
use nautilus_common::udp::tokio::AsyncUdpSocketExt;
use std::net::{IpAddr, Ipv4Addr, SocketAddr};
mod command;
mod telemetry;
use crate::command::CommandHandler;
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::time::Duration;
use std::thread::{Builder, scope};
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 bind_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), 14000);
let udp = UdpSocket::bind(bind_addr)?;
udp.set_read_timeout(Some(Duration::from_millis(100)))?;
let udp_thread = Builder::new()
.name("flight-udp-connection-handler".to_string())
.spawn(move || {
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) {
match udp.recv_postcard::<Telemetry>(&mut buffer) {
Ok((tlm, addr)) => {
flight_addr = Some(addr);
info!("{tlm:?}");
Ok(()) as Result<()>
})?;
if do_once {
do_once = false;
udp.send_command(
"/mcp23017a/set",
&ValidPriorityCommand {
inner: SetPin {
pin: 0,
value: true,
},
valid_until: Utc::now() + TimeDelta::seconds(5),
priority: 0,
},
addr,
)?;
}
}
Err(UdpRecvPostcardError::NoData) => {
// NoOp
}
Err(err) => {
error!("Rx error: {err}");
}
}
let f1 = cancel.cancelled();
pin!(f1);
let udp = block_on(async {
let f2 = udp;
select(f1, f2).await
});
let udp = match udp {
Either::Left(_) => bail!("Cancelled Before UDP established"),
Either::Right(x) => x.0?,
};
(udp_thread, udp)
};
let flight_addr = RwLock::new(SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), 0));
let lock = flight_addr.blocking_write();
scope(|scope| {
let client = Arc::new(Client::connect("ws://localhost:8080/backend")?);
let tlm = TelemetryHandler::new(client.clone(), &flight_addr, lock, &udp, cancel.clone());
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.send_command("/shutdown", &(), flight_addr)?;
// 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])?;
}
udp_shutdown.cancel();
udp_thread.join().map_err(|e| anyhow!("{e:?}"))??;
Ok(())
}

142
ground/src/telemetry.rs Normal file
View File

@@ -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;
}
}
}
}