From b067ae5cecfb306c5b69a7b71a2709b411b64d34 Mon Sep 17 00:00:00 2001 From: Sergey Savelyev Date: Sat, 25 Oct 2025 12:23:23 -0700 Subject: [PATCH] initial comms --- Cargo.lock | 350 ++++++++------------------- common/Cargo.toml | 8 + common/src/command/mod.rs | 6 + common/src/lib.rs | 24 +- {flight => common}/src/logger.rs | 4 +- common/src/telemetry/mod.rs | 8 + common/src/udp.rs | 80 ++++++ flight/Cargo.toml | 7 +- flight/src/comms/mod.rs | 64 +++++ flight/src/hardware/mcp23017/task.rs | 126 +++++----- flight/src/hardware/raspi/mod.rs | 1 + flight/src/lib.rs | 29 +-- flight/src/main.rs | 5 +- flight/src/scheduler/mod.rs | 114 +++++++++ ground/Cargo.toml | 4 + ground/src/lib.rs | 50 ++++ ground/src/main.rs | 12 +- 17 files changed, 536 insertions(+), 356 deletions(-) create mode 100644 common/src/command/mod.rs rename {flight => common}/src/logger.rs (94%) create mode 100644 common/src/telemetry/mod.rs create mode 100644 common/src/udp.rs create mode 100644 flight/src/comms/mod.rs create mode 100644 flight/src/scheduler/mod.rs create mode 100644 ground/src/lib.rs diff --git a/Cargo.lock b/Cargo.lock index 7d28ee4..8d17aff 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -17,15 +17,6 @@ version = "1.0.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a23eb6b1614318a8071c9b2521f36b424b2c83db5eb3a0fead4a6c0809af6e61" -[[package]] -name = "approx" -version = "0.5.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" -dependencies = [ - "num-traits", -] - [[package]] name = "autocfg" version = "1.4.0" @@ -44,12 +35,6 @@ version = "3.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf" -[[package]] -name = "bytemuck" -version = "1.23.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9134a6ef01ce4b366b50689c94f82c14bc72bc5d0386829828a2e2752ef7958c" - [[package]] name = "cc" version = "1.2.17" @@ -80,10 +65,38 @@ dependencies = [ "iana-time-zone", "js-sys", "num-traits", + "serde", "wasm-bindgen", "windows-link", ] +[[package]] +name = "ciborium" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "42e69ffd6f0917f5c029256a24d0161db17cea3997d185db0d35926308770f0e" +dependencies = [ + "ciborium-io", + "ciborium-ll", + "serde", +] + +[[package]] +name = "ciborium-io" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05afea1e0a06c9be33d539b876f1ce3692f4afea2cb41f740e7743225ed1c757" + +[[package]] +name = "ciborium-ll" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57663b653d948a338bfb3eeba9bb2fd5fcfaecb9e199e87e1eda4d9e8b240fd9" +dependencies = [ + "ciborium-io", + "half", +] + [[package]] name = "colored" version = "2.2.0" @@ -121,6 +134,12 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" +[[package]] +name = "crunchy" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "460fbee9c2c2f33933d720630a6a0bac33ba7053db5344fac858d4b8952d77d5" + [[package]] name = "ctrlc" version = "3.5.0" @@ -208,100 +227,15 @@ dependencies = [ ] [[package]] -name = "glam" -version = "0.14.0" +name = "half" +version = "2.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "333928d5eb103c5d4050533cec0384302db6be8ef7d3cebd30ec6a35350353da" - -[[package]] -name = "glam" -version = "0.15.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3abb554f8ee44336b72d522e0a7fe86a29e09f839a36022fa869a7dfe941a54b" - -[[package]] -name = "glam" -version = "0.16.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4126c0479ccf7e8664c36a2d719f5f2c140fbb4f9090008098d2c291fa5b3f16" - -[[package]] -name = "glam" -version = "0.17.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e01732b97afd8508eee3333a541b9f7610f454bb818669e66e90f5f57c93a776" - -[[package]] -name = "glam" -version = "0.18.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "525a3e490ba77b8e326fb67d4b44b4bd2f920f44d4cc73ccec50adc68e3bee34" - -[[package]] -name = "glam" -version = "0.19.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b8509e6791516e81c1a630d0bd7fbac36d2fa8712a9da8662e716b52d5051ca" - -[[package]] -name = "glam" -version = "0.20.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f43e957e744be03f5801a55472f593d43fabdebf25a4585db250f04d86b1675f" - -[[package]] -name = "glam" -version = "0.21.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "518faa5064866338b013ff9b2350dc318e14cc4fcd6cb8206d7e7c9886c98815" - -[[package]] -name = "glam" -version = "0.22.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "12f597d56c1bd55a811a1be189459e8fad2bbc272616375602443bdfb37fa774" - -[[package]] -name = "glam" -version = "0.23.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e4afd9ad95555081e109fe1d21f2a30c691b5f0919c67dfa690a2e1eb6bd51c" - -[[package]] -name = "glam" -version = "0.24.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5418c17512bdf42730f9032c74e1ae39afc408745ebb2acf72fbc4691c17945" - -[[package]] -name = "glam" -version = "0.25.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "151665d9be52f9bb40fc7966565d39666f2d1e69233571b71b87791c7e0528b3" - -[[package]] -name = "glam" -version = "0.27.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9e05e7e6723e3455f4818c7b26e855439f7546cf617ef669d1adedb8669e5cb9" - -[[package]] -name = "glam" -version = "0.28.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "779ae4bf7e8421cf91c0b3b64e7e8b40b862fba4d393f59150042de7c4965a94" - -[[package]] -name = "glam" -version = "0.29.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8babf46d4c1c9d92deac9f7be466f76dfc4482b6452fc5024b5e8daf6ffeb3ee" - -[[package]] -name = "glam" -version = "0.30.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e12d847aeb25f41be4c0ec9587d624e9cd631bc007a8fd7ce3f5851e064c6460" +checksum = "6ea2d84b969582b4b1864a92dc5d27cd2b77b622a8d79306834f1be5ba20d84b" +dependencies = [ + "cfg-if", + "crunchy", + "zerocopy", +] [[package]] name = "hex" @@ -361,62 +295,19 @@ version = "0.4.28" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432" -[[package]] -name = "matrixmultiply" -version = "0.3.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08" -dependencies = [ - "autocfg", - "rawpointer", -] - -[[package]] -name = "nalgebra" -version = "0.34.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c4d5b3eff5cd580f93da45e64715e8c20a3996342f1e466599cf7a267a0c2f5f" -dependencies = [ - "approx", - "glam 0.14.0", - "glam 0.15.2", - "glam 0.16.0", - "glam 0.17.3", - "glam 0.18.0", - "glam 0.19.0", - "glam 0.20.5", - "glam 0.21.3", - "glam 0.22.0", - "glam 0.23.0", - "glam 0.24.2", - "glam 0.25.0", - "glam 0.27.0", - "glam 0.28.0", - "glam 0.29.3", - "glam 0.30.8", - "matrixmultiply", - "nalgebra-macros", - "num-complex 0.4.6", - "num-rational 0.4.2", - "num-traits", - "simba", - "typenum", -] - -[[package]] -name = "nalgebra-macros" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "nautilus_common" version = "0.1.0" +dependencies = [ + "anyhow", + "chrono", + "ciborium", + "ctrlc", + "fern", + "log", + "serde", + "thiserror", +] [[package]] name = "nautilus_flight" @@ -424,23 +315,26 @@ version = "0.0.1" dependencies = [ "anyhow", "chrono", + "ciborium", "crc", - "ctrlc", "embedded-hal 1.0.0", "embedded-hal-bus", "embedded-hal-mock", - "fern", "hex", "log", - "nalgebra", - "num-traits", + "nautilus_common", "rpi-pal", - "thiserror", ] [[package]] name = "nautilus_ground" version = "0.1.0" +dependencies = [ + "anyhow", + "ciborium", + "log", + "nautilus_common", +] [[package]] name = "nb" @@ -475,20 +369,10 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" dependencies = [ - "num-complex 0.3.1", + "num-complex", "num-integer", "num-iter", - "num-rational 0.3.2", - "num-traits", -] - -[[package]] -name = "num-bigint" -version = "0.4.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9" -dependencies = [ - "num-integer", + "num-rational", "num-traits", ] @@ -501,15 +385,6 @@ dependencies = [ "num-traits", ] -[[package]] -name = "num-complex" -version = "0.4.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" -dependencies = [ - "num-traits", -] - [[package]] name = "num-integer" version = "0.1.46" @@ -541,17 +416,6 @@ dependencies = [ "num-traits", ] -[[package]] -name = "num-rational" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" -dependencies = [ - "num-bigint", - "num-integer", - "num-traits", -] - [[package]] name = "num-traits" version = "0.2.19" @@ -567,12 +431,6 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" -[[package]] -name = "paste" -version = "1.0.15" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" - [[package]] name = "proc-macro2" version = "1.0.94" @@ -591,12 +449,6 @@ dependencies = [ "proc-macro2", ] -[[package]] -name = "rawpointer" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" - [[package]] name = "rpi-pal" version = "0.22.2" @@ -619,12 +471,33 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2" [[package]] -name = "safe_arch" -version = "0.7.4" +name = "serde" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" dependencies = [ - "bytemuck", + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn", ] [[package]] @@ -633,19 +506,6 @@ version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" -[[package]] -name = "simba" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b3a386a501cd104797982c15ae17aafe8b9261315b5d07e3ec803f2ea26be0fa" -dependencies = [ - "approx", - "num-complex 0.4.6", - "num-traits", - "paste", - "wide", -] - [[package]] name = "spin_sleep" version = "1.3.1" @@ -686,12 +546,6 @@ dependencies = [ "syn", ] -[[package]] -name = "typenum" -version = "1.18.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f" - [[package]] name = "unicode-ident" version = "1.0.18" @@ -762,16 +616,6 @@ dependencies = [ "unicode-ident", ] -[[package]] -name = "wide" -version = "0.7.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22" -dependencies = [ - "bytemuck", - "safe_arch", -] - [[package]] name = "windows-core" version = "0.52.0" @@ -868,3 +712,23 @@ name = "windows_x86_64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "zerocopy" +version = "0.8.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0894878a5fa3edfd6da3f88c4805f4c8558e2b996227a3d864f47fe11e38282c" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88d2b8d9c68ad2b9e4340d7832716a4d21a22a1154777ad56ea55c51a9cf3831" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] diff --git a/common/Cargo.toml b/common/Cargo.toml index cf3c706..f3988af 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -4,3 +4,11 @@ version = "0.1.0" edition = "2024" [dependencies] +anyhow = "1.0.100" +fern = { version = "0.7.1", features = ["colored"] } +log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] } +chrono = { version = "0.4.42", features = ["serde"] } +ctrlc = "3.5.0" +serde = { version = "1.0.228", features = ["derive"], default-features = false } +ciborium = { version = "0.2.2" } +thiserror = "2.0.17" diff --git a/common/src/command/mod.rs b/common/src/command/mod.rs new file mode 100644 index 0000000..4bd9b55 --- /dev/null +++ b/common/src/command/mod.rs @@ -0,0 +1,6 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub enum Command { + Shutdown, +} diff --git a/common/src/lib.rs b/common/src/lib.rs index b93cf3f..47f786a 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -1,14 +1,16 @@ -pub fn add(left: u64, right: u64) -> u64 { - left + right -} +use log::info; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; -#[cfg(test)] -mod tests { - use super::*; +pub mod logger; +pub mod command; +pub mod telemetry; +pub mod udp; - #[test] - fn it_works() { - let result = add(2, 2); - assert_eq!(result, 4); - } +pub fn add_ctrlc_handler(flag: Arc) -> anyhow::Result<()> { + ctrlc::set_handler(move || { + info!("Shutdown Requested"); + flag.store(false, Ordering::Relaxed); + })?; + Ok(()) } diff --git a/flight/src/logger.rs b/common/src/logger.rs similarity index 94% rename from flight/src/logger.rs rename to common/src/logger.rs index 551277b..6034774 100644 --- a/flight/src/logger.rs +++ b/common/src/logger.rs @@ -5,13 +5,13 @@ use std::fs::create_dir_all; use std::str::FromStr; use std::{env, thread}; -pub fn setup_logger() -> Result<()> { +pub fn setup_logger(package_name: &'static str) -> Result<()> { let log_file = env::var("LOG_FILE").or_else(|_| { create_dir_all("logs/")?; anyhow::Ok(format!( "logs/{}_{}.log", - env!("CARGO_PKG_NAME"), + package_name, chrono::Local::now().format("%Y%m%d_%H%M%S") )) })?; diff --git a/common/src/telemetry/mod.rs b/common/src/telemetry/mod.rs new file mode 100644 index 0000000..bc442e0 --- /dev/null +++ b/common/src/telemetry/mod.rs @@ -0,0 +1,8 @@ +use chrono::serde::ts_nanoseconds; +use chrono::{DateTime, Utc}; +use serde::{Deserialize, Serialize}; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub enum Telemetry { + Timestamp(#[serde(with = "ts_nanoseconds")] DateTime) +} diff --git a/common/src/udp.rs b/common/src/udp.rs new file mode 100644 index 0000000..eba9833 --- /dev/null +++ b/common/src/udp.rs @@ -0,0 +1,80 @@ +use crate::udp::UdpSendCborError::LengthMismatch; +use log::error; +use serde::de::DeserializeOwned; +use serde::Serialize; +use std::io::{Cursor, ErrorKind}; +use std::net::{SocketAddr, ToSocketAddrs, UdpSocket}; +use thiserror::Error; + +#[derive(Error, Debug)] +pub enum UdpRecvCborError { + #[error("IO Error")] + Io(#[from] std::io::Error), + #[error("Deserialization Error")] + Deserialization(#[from] ciborium::de::Error), + #[error("No Data")] + NoData, +} + +#[derive(Error, Debug)] +pub enum UdpSendCborError { + #[error("IO Error")] + Io(#[from] std::io::Error), + #[error("Serialization Error")] + Serialization(#[from] ciborium::ser::Error), + #[error("Length Mismatch")] + LengthMismatch { + expected: usize, + actual: usize, + }, +} + +pub trait UdpSocketExt { + fn recv_cbor(&self, buffer: &mut Cursor<[u8; N]>) -> Result<(T, SocketAddr), UdpRecvCborError>; + fn send_cbor(&self, data: &T, buffer: &mut Cursor<[u8; N]>, addr: A) -> Result<(), UdpSendCborError>; +} + +impl UdpSocketExt for UdpSocket { + fn recv_cbor(&self, buffer: &mut Cursor<[u8; N]>) -> Result<(T, SocketAddr), UdpRecvCborError> { + buffer.set_position(0); + match self.recv_from(buffer.get_mut()) { + Ok((size, addr)) => { + match ciborium::from_reader::(&buffer.get_ref()[..size]) { + Ok(res) => Ok((res, addr)), + Err(err) => Err(err.into()), + } + } + Err(err) => { + match err.kind() { + ErrorKind::WouldBlock | ErrorKind::TimedOut => { + Err(UdpRecvCborError::NoData) + } + _ => Err(err.into()) + } + } + } + } + + fn send_cbor(&self, data: &T, mut buffer: &mut Cursor<[u8; N]>, addr: A) -> Result<(), UdpSendCborError> { + buffer.set_position(0); + match ciborium::into_writer(data, &mut buffer) { + Ok(_) => match self.send_to(&buffer.get_ref()[..buffer.position() as usize], addr) { + Ok(size) => { + if buffer.position() as usize != size { + return Err(LengthMismatch { + expected: buffer.position() as usize, + actual: size, + }); + } + Ok(()) + } + Err(e) => { + Err(e.into()) + } + } + Err(e) => { + Err(e.into()) + } + } + } +} diff --git a/flight/Cargo.toml b/flight/Cargo.toml index ac7d34f..f2177fc 100644 --- a/flight/Cargo.toml +++ b/flight/Cargo.toml @@ -5,19 +5,16 @@ edition = "2024" [dependencies] anyhow = "1.0.100" -fern = { version = "0.7.1", features = ["colored"] } log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] } chrono = "0.4.42" 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 } -nalgebra = "0.34.1" hex = "0.4.3" -thiserror = "2.0.17" -num-traits = "0.2.19" crc = "3.3.0" -ctrlc = { version = "3.5" } +nautilus_common = { path = "../common" } +ciborium = { version = "0.2.2" } [dev-dependencies] embedded-hal-mock = { version = "0.11.1" } diff --git a/flight/src/comms/mod.rs b/flight/src/comms/mod.rs new file mode 100644 index 0000000..f115dc1 --- /dev/null +++ b/flight/src/comms/mod.rs @@ -0,0 +1,64 @@ +use crate::scheduler::CyclicTask; +use anyhow::Result; +use log::{error, trace}; +use nautilus_common::command::Command; +use nautilus_common::telemetry::Telemetry; +use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; +use std::fmt::Debug; +use std::io::Cursor; +use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket}; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::mpsc::Receiver; +use std::sync::Arc; +use std::time::Instant; + +#[derive(Debug)] +pub struct CommsTask { + udp: UdpSocket, + ground_address: A, + running: Arc, +} + +impl CommsTask { + pub fn new( + local_port: u16, + ground_address: A, + running: Arc, + ) -> Result { + trace!("CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})"); + let bind_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), local_port); + // let bind_addr = SocketAddr::new(IpAddr::V6(Ipv6Addr::UNSPECIFIED), local_port); + let udp = UdpSocket::bind(bind_addr)?; + udp.set_nonblocking(true)?; + Ok(Self { + udp, + ground_address, + running, + }) + } +} + +impl CyclicTask for CommsTask { + type Message = (); + + fn step(&mut self, _receiver: &Receiver, _step_time: Instant) { + let mut buffer = Cursor::new([0u8; 512]); + + match self.udp.recv_cbor::(&mut buffer) { + Ok((cmd, _)) => { + match cmd { + Command::Shutdown => self.running.store(false, Ordering::Relaxed), + } + } + Err(UdpRecvCborError::NoData) => {} + Err(err) => { + error!("Rx error: {err}"); + } + } + + let tlm = Telemetry::Timestamp(chrono::Utc::now()); + if let Err(err) = self.udp.send_cbor(&tlm, &mut buffer, &self.ground_address) { + error!("Tx Error: {err}"); + } + } +} diff --git a/flight/src/hardware/mcp23017/task.rs b/flight/src/hardware/mcp23017/task.rs index 4a4c1c7..6de9511 100644 --- a/flight/src/hardware/mcp23017/task.rs +++ b/flight/src/hardware/mcp23017/task.rs @@ -1,14 +1,11 @@ use crate::hardware::mcp23017::Mcp23017; use crate::hardware::pin::PinDevice; -use anyhow::Result; +use crate::scheduler::{CyclicTask, TaskHandle}; use embedded_hal::digital::PinState; use log::trace; -use std::fmt::Debug; -use std::sync::atomic::{AtomicBool, Ordering}; -use std::sync::mpsc::{channel, Sender}; -use std::thread; -use std::thread::{sleep, Scope, ScopedJoinHandle}; -use std::time::{Duration, Instant}; +use std::fmt::{Debug, Formatter}; +use std::sync::mpsc::Receiver; +use std::time::Instant; #[derive(Clone, Debug)] pub enum Mcp23017Message { @@ -20,14 +17,7 @@ pub enum Mcp23017Message { }, } -#[derive(Clone, Debug)] -pub struct Mcp23017Task { - #[allow(dead_code)] - name: String, - sender: Sender, -} - -impl PinDevice for Mcp23017Task { +impl PinDevice for TaskHandle { fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) { trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})"); // This can only fail if the other end is disconnected - which we intentionally want to @@ -41,6 +31,17 @@ impl PinDevice for Mcp23017Task { } } +pub struct Mcp23017Task { + mcp23017: M, + pins: AllPins, +} + +impl Debug for Mcp23017Task { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + write!(f, "Mcp23017Task {{ mcp23017: {:?} }}", self.mcp23017) + } +} + struct AllPins { pins: [PinData; 16], } @@ -137,67 +138,48 @@ impl PinData { } } -impl Mcp23017Task { - pub fn start<'a, M: Mcp23017 + Send + Debug>( - scope: &'a Scope<'a, '_>, - running: &'a AtomicBool, - mut mcp23017: M, - name: String, - frequency: u64, - ) -> Result<(ScopedJoinHandle<'a, ()>, Self)> where - M: 'a, - { - trace!("Mcp23017Task::start(scope, running, mcp23017: {mcp23017:?}, name: {name}, frequency: {frequency})"); +impl Mcp23017Task { + pub fn new(mcp23017: M) -> Self { + trace!("Mcp23017Task::new(mcp23017: {mcp23017:?})"); + Self { + mcp23017, + pins: AllPins::new(), + } + } +} - let (sender, receiver) = channel::(); - let period = Duration::from_nanos(1_000_000_000 / frequency); +impl CyclicTask for Mcp23017Task { + type Message = Mcp23017Message; - let handle = thread::Builder::new() - .name(name.clone()) - .spawn_scoped(scope, move || { - let mut pins = AllPins::new(); - let mut cycle_start_time = Instant::now(); - while running.load(Ordering::Relaxed) { - let mut changed = false; + fn step( + &mut self, + receiver: &Receiver, + step_time: Instant, + ) { + let mut changed = false; - while let Ok(recv) = receiver.try_recv() { - match recv { - Mcp23017Message::SetPin { pin, value, valid_until, priority } => { - if (0u8..16u8).contains(&pin) { - pins.pins[pin as usize].set(value, valid_until, priority); - } - } - } - } - - for pin in 0u8..16u8 { - // This shouldn't be able to fail - // TODO: handle error case - let state = pins.pins[pin as usize].get(cycle_start_time); - if pins.pins[pin as usize].changed { - pins.pins[pin as usize].changed = false; - let _ = mcp23017.set_pin(pin, state); - changed = true; - } - } - if changed { - let _ = mcp23017.flush(); - } - - cycle_start_time += period; - let sleep_duration = cycle_start_time - Instant::now(); - if sleep_duration > Duration::ZERO { - sleep(sleep_duration); + while let Ok(recv) = receiver.try_recv() { + match recv { + Mcp23017Message::SetPin { pin, value, valid_until, priority } => { + if (0u8..16u8).contains(&pin) { + self.pins.pins[pin as usize].set(value, valid_until, priority); } } - })?; - - Ok(( - handle, - Self { - name, - sender, } - )) + } + + for pin in 0u8..16u8 { + // This shouldn't be able to fail + // TODO: handle error case + let state = self.pins.pins[pin as usize].get(step_time); + if self.pins.pins[pin as usize].changed { + self.pins.pins[pin as usize].changed = false; + let _ = self.mcp23017.set_pin(pin, state); + changed = true; + } + } + if changed { + let _ = self.mcp23017.flush(); + } } -} \ No newline at end of file +} diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index bd68175..0430024 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -44,6 +44,7 @@ impl RaspiHardware { Mode::Mode0, )?), 3.3f64).into(), mct8316a: SimMct8316a::new().into(), + // mct8316a: SimMct8316a::new().into(), }) } } diff --git a/flight/src/lib.rs b/flight/src/lib.rs index cc0ace4..7ce4a0e 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -1,30 +1,23 @@ +use crate::comms::CommsTask; use crate::hardware::channelization::{LED_A, LED_B}; use crate::hardware::initialize; use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task}; use crate::hardware::mct8316a::Mct8316a; use crate::hardware::pin::Pin; use crate::hardware::Hardware; -use crate::on_drop::on_drop; +use crate::scheduler::Scheduler; use anyhow::Result; use embedded_hal::digital::PinState; use embedded_hal::pwm::SetDutyCycle; use log::{debug, info}; +use nautilus_common::add_ctrlc_handler; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; -use std::thread; use std::thread::sleep; use std::time::{Duration, Instant}; mod hardware; -fn add_ctrlc_handler(flag: Arc) -> Result<()> { - ctrlc::set_handler(move || { - info!("Shutdown Requested"); - flag.store(false, Ordering::Relaxed); - })?; - Ok(()) -} - pub fn run() -> Result<()> { info!( "Project Nautilus Flight Software {}", @@ -49,21 +42,17 @@ pub fn run() -> Result<()> { mcp23017_b.init()?; mct8316.init()?; - thread::scope(|scope| { - // This will automatically set running to false when it drops - // This means that if the main thread exits this scope, we will - // shut down any side branches - let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed)); + Scheduler::new(running.clone(), |s| { + let task_a = s.run_cyclic("mcp23017-a", Mcp23017Task::new(mcp23017_a), 10)?; + let task_b = s.run_cyclic("mcp23017-b", Mcp23017Task::new(mcp23017_b), 10)?; - let (_, task_a) = Mcp23017Task::start(scope, &running, mcp23017_a, "mcp23017-a".into(), 10)?; - - let (_, task_b) = Mcp23017Task::start(scope, &running, mcp23017_b, "mcp23017-b".into(), 10)?; + let _comms = s.run_cyclic("comms", CommsTask::new(15000, "192.168.50.157:14000", running.clone())?, 1)?; let mut led_pin_a = LED_A.new(&task_a, &task_b)?; let mut led_pin_b = LED_B.new(&task_a, &task_b)?; info!("Starting Main Loop"); - for _ in 0..2 { + loop { debug!("A On"); led_pin_a.set(PinState::High, Instant::now() + Duration::from_secs(2), 0); sleep(Duration::from_secs(1)); @@ -103,3 +92,5 @@ mod test_utils; mod data; mod on_drop; mod rcs; +mod comms; +mod scheduler; diff --git a/flight/src/main.rs b/flight/src/main.rs index fba2f42..5661f8b 100644 --- a/flight/src/main.rs +++ b/flight/src/main.rs @@ -1,11 +1,10 @@ -use crate::logger::setup_logger; use log::error; +use nautilus_common::logger::setup_logger; use nautilus_flight::run; -mod logger; fn main() { - setup_logger().expect("Failed to setup logger"); + setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger"); match run() { Ok(_) => {} Err(err) => { diff --git a/flight/src/scheduler/mod.rs b/flight/src/scheduler/mod.rs new file mode 100644 index 0000000..7c65f61 --- /dev/null +++ b/flight/src/scheduler/mod.rs @@ -0,0 +1,114 @@ +use crate::on_drop::on_drop; +use anyhow::Result; +use log::trace; +use std::fmt::Debug; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::mpsc::{channel, Receiver, Sender}; +use std::sync::Arc; +use std::thread; +use std::thread::{sleep, Scope}; +use std::time::{Duration, Instant}; + +#[derive(Clone, Debug)] +pub struct TaskHandle { + #[allow(dead_code)] + pub name: String, + pub sender: Sender, +} + +#[allow(dead_code)] +pub trait Task { + type Message; + + fn run(self, receiver: Receiver, running: Arc); +} + +pub trait CyclicTask { + type Message; + + fn step(&mut self, receiver: &Receiver, step_time: Instant); +} + +pub struct Scheduler<'s, 'e> +{ + scope: &'s Scope<'s, 'e>, + running: Arc, +} + +impl<'s, 'e> Scheduler<'s, 'e> { + pub fn new<'env, F, R>(running: Arc, f: F) -> R + where + F: FnOnce(Scheduler<'_, 'env>) -> R, + { + trace!("Scheduler::new(running: {running:?}, f)"); + thread::scope(|scope: &Scope| { + // This will automatically set running to false when it drops + // This means that if the function returns any side branches + // checking running will shut down + let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed)); + + f(Scheduler { + scope, + running: running.clone(), + }) + }) + } + + #[allow(dead_code)] + pub fn run( + &self, + name: impl Into, + task: T, + ) -> Result> where + T: Task + Send + Debug + 's, + T::Message: Send, + { + let name = name.into(); + trace!("Scheduler::run(name: {name}, task: {task:?})"); + let running = self.running.clone(); + let (sender, receiver) = channel::(); + let _ = thread::Builder::new() + .name(name.clone()) + .spawn_scoped(self.scope, move || { + task.run(receiver, running); + })?; + Ok(TaskHandle { + name, + sender, + }) + } + + pub fn run_cyclic( + &self, + name: impl Into, + mut task: T, + frequency: u64, + ) -> Result> where + T: CyclicTask + Send + Debug + 's, + T::Message: Send, + { + let name = name.into(); + trace!("Scheduler::run_cyclic(name: {name}, task: {task:?}, frequency: {frequency})"); + let running = self.running.clone(); + let (sender, receiver) = channel::(); + let _ = thread::Builder::new() + .name(name.clone()) + .spawn_scoped(self.scope, move || { + let period = Duration::from_nanos(1_000_000_000 / frequency); + let mut cycle_start_time = Instant::now(); + while running.load(Ordering::Relaxed) { + task.step(&receiver, cycle_start_time); + + cycle_start_time += period; + let sleep_duration = cycle_start_time - Instant::now(); + if sleep_duration > Duration::ZERO { + sleep(sleep_duration); + } + } + })?; + Ok(TaskHandle { + name, + sender, + }) + } +} diff --git a/ground/Cargo.toml b/ground/Cargo.toml index f6f0ae0..725c0ea 100644 --- a/ground/Cargo.toml +++ b/ground/Cargo.toml @@ -4,3 +4,7 @@ version = "0.1.0" edition = "2024" [dependencies] +anyhow = "1.0.100" +nautilus_common = { path = "../common" } +log = "0.4.28" +ciborium = { version = "0.2.2" } diff --git a/ground/src/lib.rs b/ground/src/lib.rs new file mode 100644 index 0000000..97b6b52 --- /dev/null +++ b/ground/src/lib.rs @@ -0,0 +1,50 @@ +use anyhow::Result; +use log::{error, info}; +use nautilus_common::add_ctrlc_handler; +use nautilus_common::command::Command; +use nautilus_common::telemetry::Telemetry; +use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; +use std::io::Cursor; +use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket}; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; +use std::time::Duration; + +pub fn run() -> Result<()> { + info!( + "Project Nautilus Ground Software {}", + env!("CARGO_PKG_VERSION") + ); + + let running = Arc::new(AtomicBool::new(true)); + + add_ctrlc_handler(running.clone())?; + + 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 mut buffer = Cursor::new([0u8; 512]); + while running.load(Ordering::Relaxed) { + match udp.recv_cbor::(&mut buffer) { + Ok((tlm, addr)) => { + flight_addr = Some(addr); + info!("{tlm:?}"); + } + Err(UdpRecvCborError::NoData) => { + // NoOp + } + Err(err) => { + error!("Rx error: {err}"); + } + } + } + + if let Some(flight_addr) = flight_addr { + let cmd = Command::Shutdown; + udp.send_cbor(&cmd, &mut buffer, &flight_addr)?; + } + + Ok(()) +} \ No newline at end of file diff --git a/ground/src/main.rs b/ground/src/main.rs index e7a11a9..b09f287 100644 --- a/ground/src/main.rs +++ b/ground/src/main.rs @@ -1,3 +1,13 @@ +use log::error; +use nautilus_common::logger::setup_logger; +use nautilus_ground::run; + fn main() { - println!("Hello, world!"); + setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger"); + match run() { + Ok(_) => {} + Err(err) => { + error!("An unhandled error occurred: {}\n\n{}", err, err.backtrace()); + } + } }