initial integration with telem viz
This commit is contained in:
@@ -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(())
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user