diff --git a/Cargo.lock b/Cargo.lock index 7a6c778..01e3db3 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,6 +2,15 @@ # It is not intended for manual editing. version = 4 +[[package]] +name = "aho-corasick" +version = "1.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301" +dependencies = [ + "memchr", +] + [[package]] name = "android_system_properties" version = "0.1.5" @@ -11,6 +20,56 @@ dependencies = [ "libc", ] +[[package]] +name = "anstream" +version = "0.6.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43d5b281e737544384e969a5ccad3f1cdd24b48086a0fc1b2a5262a26b8f4f4a" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + +[[package]] +name = "anstyle" +version = "1.0.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5192cca8006f1fd4f7237516f40fa183bb07f8fbdfedaa0036de5ea9b0b45e78" + +[[package]] +name = "anstyle-parse" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e7644824f0aa2c7b9384579234ef10eb7efb6a0deb83f9630a49594dd9c15c2" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40c48f72fd53cd289104fc64099abca73db4166ad86ea0b4341abe65af83dadc" +dependencies = [ + "windows-sys 0.61.1", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291e6a250ff86cd4a820112fb8898808a366d8f9f58ce16d1f538353ad55747d" +dependencies = [ + "anstyle", + "once_cell_polyfill", + "windows-sys 0.61.1", +] + [[package]] name = "anyhow" version = "1.0.100" @@ -145,6 +204,12 @@ dependencies = [ "thiserror", ] +[[package]] +name = "colorchoice" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75" + [[package]] name = "colored" version = "2.2.0" @@ -359,6 +424,29 @@ dependencies = [ "num", ] +[[package]] +name = "env_filter" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1bf3c259d255ca70051b30e2e95b5446cdb8949ac4cd22c0d7fd634d89f568e2" +dependencies = [ + "log", + "regex", +] + +[[package]] +name = "env_logger" +version = "0.11.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "13c863f0904021b108aa8b2f55046443e6b1ebde8fd4a15c399893aae4fa069f" +dependencies = [ + "anstream", + "anstyle", + "env_filter", + "jiff", + "log", +] + [[package]] name = "fern" version = "0.7.1" @@ -369,6 +457,12 @@ dependencies = [ "log", ] +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + [[package]] name = "futures" version = "0.3.31" @@ -491,6 +585,16 @@ dependencies = [ "wasip2", ] +[[package]] +name = "good_lp" +version = "1.14.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "776aa1ba88ac058e78408c17f4dbff826a51ae08ed6642f71ca0edd7fe9383f3" +dependencies = [ + "fnv", + "microlp", +] + [[package]] name = "http" version = "1.4.0" @@ -531,6 +635,12 @@ dependencies = [ "cc", ] +[[package]] +name = "is_terminal_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" + [[package]] name = "itertools" version = "0.14.0" @@ -546,6 +656,30 @@ version = "1.0.17" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "92ecc6618181def0457392ccd0ee51198e065e016d1d527a7ac1b6dc7c1f09d2" +[[package]] +name = "jiff" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a87d9b8105c23642f50cbbae03d1f75d8422c5cb98ce7ee9271f7ff7505be6b8" +dependencies = [ + "jiff-static", + "log", + "portable-atomic", + "portable-atomic-util", + "serde_core", +] + +[[package]] +name = "jiff-static" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b787bebb543f8969132630c51fd0afab173a86c6abae56ff3b9e5e3e3f9f6e58" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.112", +] + [[package]] name = "js-sys" version = "0.3.77" @@ -574,12 +708,32 @@ version = "0.4.29" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897" +[[package]] +name = "matrixmultiply" +version = "0.3.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08" +dependencies = [ + "autocfg", + "rawpointer", +] + [[package]] name = "memchr" version = "2.7.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f52b00d39961fc5b2736ea853c9cc86238e165017a493d1d5c8eac6bdc4cc273" +[[package]] +name = "microlp" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "51d1790c73b93164ff65868f63164497cb32339458a9297e17e212d91df62258" +dependencies = [ + "log", + "sprs", +] + [[package]] name = "mio" version = "1.1.1" @@ -618,6 +772,8 @@ dependencies = [ "embedded-hal 1.0.0", "embedded-hal-bus", "embedded-hal-mock", + "env_logger", + "good_lp", "log", "nautilus_common", "postcard", @@ -655,6 +811,21 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" +[[package]] +name = "ndarray" +version = "0.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c7c9125e8f6f10c9da3aad044cc918cf8784fa34de857b1aa68038eb05a50a9" +dependencies = [ + "matrixmultiply", + "num-complex 0.4.6", + "num-integer", + "num-traits", + "portable-atomic", + "portable-atomic-util", + "rawpointer", +] + [[package]] name = "nix" version = "0.30.1" @@ -673,7 +844,7 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" dependencies = [ - "num-complex", + "num-complex 0.3.1", "num-integer", "num-iter", "num-rational", @@ -689,6 +860,15 @@ 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" @@ -750,6 +930,12 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" +[[package]] +name = "once_cell_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe" + [[package]] name = "openssl-probe" version = "0.2.0" @@ -768,6 +954,21 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" +[[package]] +name = "portable-atomic" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f89776e4d69bb58bc6993e99ffa1d11f228b839984854c7daeb5d37f87cbe950" + +[[package]] +name = "portable-atomic-util" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d8a2f0d8d040d7848a709caf78912debcc3f33ee4b3cac47d73d1e1069e83507" +dependencies = [ + "portable-atomic", +] + [[package]] name = "postcard" version = "1.1.3" @@ -866,6 +1067,41 @@ dependencies = [ "getrandom 0.3.4", ] +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + +[[package]] +name = "regex" +version = "1.12.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "843bc0191f75f3e22651ae5f1e72939ab2f72a4bc30fa80a066bd66edefc24d4" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5276caf25ac86c8d810222b3dbb938e512c55c6831a10f3e6ed1c93b84041f1c" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a2d987857b319362043e95f5353c0535c1f58eec5336fdfcf626430af7def58" + [[package]] name = "ring" version = "0.17.14" @@ -1059,6 +1295,12 @@ version = "0.4.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7a2ae44ef20feb57a68b23d846850f861394c2e02dc425a50098ae8c90267589" +[[package]] +name = "smallvec" +version = "1.15.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03" + [[package]] name = "socket2" version = "0.6.1" @@ -1078,6 +1320,18 @@ dependencies = [ "windows-sys 0.59.0", ] +[[package]] +name = "sprs" +version = "0.11.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6dca58a33be2188d4edc71534f8bafa826e787cc28ca1c47f31be3423f0d6e55" +dependencies = [ + "ndarray", + "num-complex 0.4.6", + "num-traits", + "smallvec", +] + [[package]] name = "subtle" version = "2.6.1" @@ -1245,6 +1499,12 @@ version = "0.7.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "09cc8ee72d2a9becf2f2febe0205bbed8fc6615b7cb429ad062dc7b7ddd036a9" +[[package]] +name = "utf8parse" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" + [[package]] name = "uuid" version = "1.19.0" diff --git a/Cargo.toml b/Cargo.toml index 9e66d1a..e4aeca3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -23,4 +23,8 @@ serde = { version = "1.0.228", features = ["derive"], default-features = false } thiserror = "2.0.17" tokio = { version = "1.48.0" } tokio-util = "0.7.17" +env_logger = "0.11.8" +good_lp = { version = "1.14.2", default-features = false } +[profile.dev.package.good_lp] +opt-level = 3 diff --git a/common/Cargo.toml b/common/Cargo.toml index fabb439..8ecf2e0 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -7,7 +7,7 @@ edition = "2024" anyhow = { workspace = true } chrono = { workspace = true } ctrlc = { workspace = true } -derive_more = {workspace = true, features = ["display", "from"]} +derive_more = {workspace = true, features = ["display", "from", "add", "add_assign", "not"]} fern = { workspace = true } log = { workspace = true } postcard = { workspace = true } diff --git a/common/src/command/mod.rs b/common/src/command/mod.rs index 1af6274..3c7ac45 100644 --- a/common/src/command/mod.rs +++ b/common/src/command/mod.rs @@ -1,48 +1,10 @@ -use chrono::serde::ts_nanoseconds; -use chrono::{DateTime, TimeDelta, Utc}; use serde::de::DeserializeOwned; use serde::{Deserialize, Serialize}; use std::fmt::Debug; -use std::ops::{Deref, DerefMut}; -use std::time::Instant; -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct SetPin { - pub pin: u8, - pub value: bool, -} - -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct ValidPriorityCommand -where - T: Clone + Debug, -{ - pub inner: T, - #[serde(with = "ts_nanoseconds")] - pub valid_until: DateTime, - pub priority: u8, -} - -impl ValidPriorityCommand -where - T: Clone + Debug, -{ - /// Get the valid until time as an Instant - /// - /// # Panics - /// While this theoretically could panic, there are checks to prevent this. - pub fn get_valid_until_instant(&self) -> Instant { - let delta = self.valid_until.signed_duration_since(Utc::now()); - let now = Instant::now(); - if delta >= TimeDelta::zero() { - // Unwrap is safe because we checked that it is not negative - now + delta.to_std().unwrap() - } else { - // Unwrap is safe because we converted the negative to a positive - now.checked_sub((-delta).to_std().unwrap()).unwrap_or(now) - } - } -} +pub mod set_pin; +pub mod set_rcs; +pub mod valid_priority_command; #[derive(Clone, Debug, Serialize, Deserialize)] pub struct CommandHeader<'a> { @@ -59,26 +21,3 @@ pub struct OwnedCommandHeader { pub trait Command: Serialize + DeserializeOwned {} impl Command for () {} - -impl Command for SetPin {} - -impl Command - for ValidPriorityCommand -{ -} - -impl Deref for ValidPriorityCommand { - type Target = T; - - fn deref(&self) -> &Self::Target { - &self.inner - } -} - -impl DerefMut - for ValidPriorityCommand -{ - fn deref_mut(&mut self) -> &mut Self::Target { - &mut self.inner - } -} diff --git a/common/src/command/set_pin.rs b/common/src/command/set_pin.rs new file mode 100644 index 0000000..883c970 --- /dev/null +++ b/common/src/command/set_pin.rs @@ -0,0 +1,10 @@ +use crate::command::Command; +use serde::{Deserialize, Serialize}; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SetPin { + pub pin: u8, + pub value: bool, +} + +impl Command for SetPin {} diff --git a/common/src/command/set_rcs.rs b/common/src/command/set_rcs.rs new file mode 100644 index 0000000..2728129 --- /dev/null +++ b/common/src/command/set_rcs.rs @@ -0,0 +1,11 @@ +use crate::command::Command; +use crate::math::Vector; +use serde::{Deserialize, Serialize}; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SetRcs { + pub translation: Vector, + pub rotation: Vector, +} + +impl Command for SetRcs {} diff --git a/common/src/command/valid_priority_command.rs b/common/src/command/valid_priority_command.rs new file mode 100644 index 0000000..691d543 --- /dev/null +++ b/common/src/command/valid_priority_command.rs @@ -0,0 +1,61 @@ +use crate::command::Command; +use chrono::serde::ts_nanoseconds; +use chrono::{DateTime, TimeDelta, Utc}; +use serde::de::DeserializeOwned; +use serde::{Deserialize, Serialize}; +use std::fmt::Debug; +use std::ops::{Deref, DerefMut}; +use std::time::Instant; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct ValidPriorityCommand +where + T: Clone + Debug, +{ + pub inner: T, + #[serde(with = "ts_nanoseconds")] + pub valid_until: DateTime, + pub priority: u8, +} + +impl Command + for ValidPriorityCommand +{ +} + +impl ValidPriorityCommand +where + T: Clone + Debug, +{ + /// Get the valid until time as an Instant + /// + /// # Panics + /// While this theoretically could panic, there are checks to prevent this. + pub fn get_valid_until_instant(&self) -> Instant { + let delta = self.valid_until.signed_duration_since(Utc::now()); + let now = Instant::now(); + if delta >= TimeDelta::zero() { + // Unwrap is safe because we checked that it is not negative + now + delta.to_std().unwrap() + } else { + // Unwrap is safe because we converted the negative to a positive + now.checked_sub((-delta).to_std().unwrap()).unwrap_or(now) + } + } +} + +impl Deref for ValidPriorityCommand { + type Target = T; + + fn deref(&self) -> &Self::Target { + &self.inner + } +} + +impl DerefMut + for ValidPriorityCommand +{ + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.inner + } +} diff --git a/common/src/lib.rs b/common/src/lib.rs index 4978fc9..75761fe 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -3,6 +3,7 @@ use log::info; pub mod command; pub mod logger; +pub mod math; pub mod on_drop; pub mod telemetry; pub mod udp; diff --git a/common/src/logger.rs b/common/src/logger.rs index e7af5bd..106972b 100644 --- a/common/src/logger.rs +++ b/common/src/logger.rs @@ -43,12 +43,13 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> { target = record.target(), )); }) + .level_for("tungstenite", LevelFilter::Warn) + .level_for("tokio_tungstenite", LevelFilter::Warn) + .level_for("microlp", LevelFilter::Warn) + .level_for("api", LevelFilter::Info) .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())?) diff --git a/common/src/math/mod.rs b/common/src/math/mod.rs new file mode 100644 index 0000000..a3ee91a --- /dev/null +++ b/common/src/math/mod.rs @@ -0,0 +1,196 @@ +use derive_more::{Add, AddAssign, Display, Neg, Sub, SubAssign}; +use serde::{Deserialize, Serialize}; +use std::ops::{Div, DivAssign, Mul, MulAssign}; + +#[derive( + Debug, + Copy, + Clone, + Add, + AddAssign, + Sub, + SubAssign, + PartialEq, + Display, + Neg, + Serialize, + Deserialize, +)] +#[display("({x}, {y}, {z})")] +pub struct Vector { + pub x: f64, + pub y: f64, + pub z: f64, +} + +impl Vector { + pub const ZERO: Vector = Vector { + x: 0.0, + y: 0.0, + z: 0.0, + }; + pub const X: Vector = Vector { + x: 1.0, + y: 0.0, + z: 0.0, + }; + pub const Y: Vector = Vector { + x: 0.0, + y: 1.0, + z: 0.0, + }; + pub const Z: Vector = Vector { + x: 0.0, + y: 0.0, + z: 1.0, + }; + + #[must_use] + #[inline] + pub const fn new(x: f64, y: f64, z: f64) -> Self { + Self { x, y, z } + } + + #[must_use] + #[inline] + pub const fn dot_product(self, rhs: Self) -> f64 { + self.x * rhs.x + self.y * rhs.y + self.z * rhs.z + } + + #[must_use] + #[inline] + pub const fn cross_product(self, rhs: Self) -> Self { + Self { + x: self.y * rhs.z - self.z * rhs.y, + y: self.z * rhs.x - self.x * rhs.z, + z: self.x * rhs.y - self.y * rhs.x, + } + } + + #[must_use] + #[inline] + pub const fn length2(self) -> f64 { + self.dot_product(self) + } + + #[must_use] + #[inline] + pub fn length(self) -> f64 { + self.length2().sqrt() + } + + #[must_use] + #[inline] + pub fn normalize(self) -> Self { + self / self.length() + } + + #[must_use] + #[inline] + pub fn with_length(self, length: f64) -> Self { + self.normalize() * length + } + + #[must_use] + #[inline] + pub const fn project_onto_vector(self, vector: Self) -> Self { + let scale_factor = self.dot_product(vector) / vector.length2(); + // Manual multiplication to allow const-ification + Self { + x: vector.x * scale_factor, + y: vector.y * scale_factor, + z: vector.z * scale_factor, + } + } + + #[must_use] + #[inline] + pub fn project_onto_plane(self, vector: Self) -> Self { + self - self.project_onto_vector(vector) + } +} + +impl Default for Vector { + #[inline] + fn default() -> Self { + Self::ZERO + } +} + +impl Mul for Vector +where + f64: Mul, + T: Copy, +{ + type Output = Self; + + #[inline] + fn mul(self, rhs: T) -> Self::Output { + Self::Output { + x: self.x * rhs, + y: self.y * rhs, + z: self.z * rhs, + } + } +} + +impl MulAssign for Vector +where + f64: MulAssign, + T: Copy, +{ + #[inline] + fn mul_assign(&mut self, rhs: T) { + self.x *= rhs; + self.y *= rhs; + self.z *= rhs; + } +} + +impl Mul for f64 { + type Output = Vector; + + #[inline] + fn mul(self, rhs: Vector) -> Self::Output { + rhs * self + } +} + +impl Div for Vector +where + f64: Div, + T: Copy, +{ + type Output = Self; + + #[inline] + fn div(self, rhs: T) -> Self::Output { + Self::Output { + x: self.x / rhs, + y: self.y / rhs, + z: self.z / rhs, + } + } +} + +impl DivAssign for Vector +where + f64: DivAssign, + T: Copy, +{ + #[inline] + fn div_assign(&mut self, rhs: T) { + self.x /= rhs; + self.y /= rhs; + self.z /= rhs; + } +} + +impl Mul for Vector { + type Output = f64; + + #[inline] + fn mul(self, rhs: Self) -> f64 { + self.dot_product(rhs) + } +} diff --git a/flight/Cargo.toml b/flight/Cargo.toml index c28e211..9c2f711 100644 --- a/flight/Cargo.toml +++ b/flight/Cargo.toml @@ -14,9 +14,11 @@ log = { workspace = true, features = ["max_level_trace", "release_max_level_debu nautilus_common = { workspace = true } postcard = { workspace = true } rpi-pal = { workspace = true, features = ["hal"], optional = true } +good_lp = { workspace = true, features = ["microlp"] } [dev-dependencies] embedded-hal-mock = { workspace = true } +env_logger = { workspace = true } [features] raspi = ["dep:rpi-pal"] diff --git a/flight/src/commanded_state.rs b/flight/src/commanded_state.rs new file mode 100644 index 0000000..0466b91 --- /dev/null +++ b/flight/src/commanded_state.rs @@ -0,0 +1,297 @@ +use log::trace; +use std::any::type_name; +use std::fmt::Debug; +use std::ops::Deref; +use std::time::Instant; + +#[derive(Debug, Clone)] +struct StateEntry { + state: T, + valid_until: Instant, + priority: u8, +} + +#[derive(Debug, Clone)] +pub struct CommandedState { + entries: [Option>; N], + default: T, + changed: bool, +} + +impl CommandedState +where + T: Debug, +{ + pub fn new(default: T) -> Self { + trace!( + "CommandedState::<{}>::new(default: {default:?})", + type_name::() + ); + Self { + entries: [const { None }; N], + default, + changed: false, + } + } + + pub fn consume_changed(&mut self) -> bool { + trace!( + "CommandedState::<{}>::consume_changed(self: {self:?})", + type_name::() + ); + let res = self.changed; + self.changed = false; + res + } + + fn evaluate_change_if(&mut self, current_time: Instant, f: F) -> &T + where + F: FnOnce(&T, &T) -> bool, + { + trace!( + "CommandedState::<{}>::evaluate_change_if(self: {self:?}, current_time: {current_time:?}, f: {})", + type_name::(), + type_name::() + ); + let mut original_value: Option> = None; + for i in 0..N { + if let Some(entry) = &self.entries[0] { + if entry.valid_until >= current_time { + if let Some(original_value) = original_value { + self.changed |= f(&original_value.state, &entry.state); + } + // SAFETY: we just checked that this index is Some + // Unfortunately we can't use entry in order to satisfy the borrow checker + return self.entries[0].as_ref().map(|x| &x.state).unwrap(); + } + // An expired entry - we need to remove it + + // Empty out this entry (keeping it in original value in case it is needed) + if i == 0 { + original_value = self.entries[0].take(); + } else { + self.entries[0] = None; + } + // Bump up all future entries + self.entries.rotate_left(1); + } else { + break; + } + } + if let Some(original_value) = original_value { + self.changed |= f(&original_value.state, &self.default); + } + &self.default + } + + fn insert_change_if(&mut self, state: T, valid_until: Instant, priority: u8, f: F) + where + F: FnOnce(&T, &T) -> bool, + { + trace!( + "CommandedState::<{}>::insert_change_if(self: {self:?}, state: {state:?}, valid_until: {valid_until:?}, priority: {priority}, f: {})", + type_name::(), + type_name::() + ); + for i in 0..N { + if let Some(entry) = &mut self.entries[i] { + // The current entry exists - let's find out it replacing it is an option + if priority >= entry.priority { + // We have enough priority to modify this entry + if priority == entry.priority || entry.valid_until <= valid_until { + // If we are same priority (always replace) or + // we will be valid for longer then we can just replace this entry + if i == 0 { + self.changed |= f(&state, &entry.state); + } + *entry = StateEntry { + state, + valid_until, + priority, + }; + // Remove entries which will expire before this one + for j in (i + 1)..N { + if let Some(later_entry) = &self.entries[j] { + // If the later entry exists and expires before this one + if later_entry.valid_until <= valid_until { + self.entries[j] = None; + self.entries[j..].rotate_left(1); + } + } + } + return; + } + // We want to add a higher priority entry, but we will expire first + if i == 0 { + self.changed |= f(&state, &entry.state); + } + self.entries[i..].rotate_right(1); + self.entries[i] = Some(StateEntry { + state, + valid_until, + priority, + }); + return; + } + } else { + // No entry exists so we can just put ourselves there + if i == 0 { + self.changed |= f(&state, &self.default); + } + self.entries[i] = Some(StateEntry { + state, + valid_until, + priority, + }); + return; + } + } + // If we got here then we didn't find a valid place to insert it + } + + #[allow(unused)] + #[inline] + pub fn evaluate_changed(&mut self, current_time: Instant) -> &T { + self.evaluate_change_if(current_time, |_, _| true) + } + + #[allow(unused)] + #[inline] + pub fn insert_changed(&mut self, state: T, valid_until: Instant, priority: u8) { + self.insert_change_if(state, valid_until, priority, |_, _| true); + } +} + +impl CommandedState +where + T: PartialEq + Debug, +{ + #[inline] + pub fn evaluate(&mut self, current_time: Instant) -> &T { + self.evaluate_change_if(current_time, T::ne) + } + + #[inline] + pub fn insert(&mut self, state: T, valid_until: Instant, priority: u8) { + self.insert_change_if(state, valid_until, priority, T::ne); + } +} + +impl Deref for CommandedState { + type Target = T; + + fn deref(&self) -> &Self::Target { + for i in 0..N { + if let Some(entry) = &self.entries[i] { + return &entry.state; + } + } + &self.default + } +} + +#[cfg(test)] +mod tests { + use super::*; + use log::LevelFilter; + use std::time::Duration; + + #[test] + fn commanded_state() { + let _ = env_logger::builder() + .is_test(true) + .filter_level(LevelFilter::Trace) + .try_init(); + + let mut state = CommandedState::<_, 2>::new(4); + let now = Instant::now(); + assert!(!state.consume_changed()); + assert_eq!(4, *state.evaluate(now)); + assert!(!state.consume_changed()); + + state.insert(5, now, 0); + assert!(state.consume_changed()); + assert!(!state.consume_changed()); + assert_eq!(5, *state.evaluate(now)); + assert!(!state.consume_changed()); + + let now = now + Duration::from_secs(1); + assert_eq!(4, *state.evaluate(now)); + assert!(state.consume_changed()); + + state.insert(6, now + Duration::from_secs(2), 0); + assert!(state.consume_changed()); + assert_eq!(6, *state.evaluate(now)); + assert!(!state.consume_changed()); + + state.insert(7, now + Duration::from_secs(1), 1); + assert!(state.consume_changed()); + assert_eq!(7, *state); + + state.insert(8, now + Duration::from_secs(3), 0); + assert!(!state.consume_changed()); + assert_eq!(7, *state); + + state.insert(9, now + Duration::from_secs(2), 1); + assert!(state.consume_changed()); + assert_eq!(9, *state); + + assert_eq!(9, *state.evaluate(now)); + assert!(!state.consume_changed()); + + let now = now + Duration::from_secs(2); + assert_eq!(9, *state.evaluate(now)); + assert!(!state.consume_changed()); + + let now = now + Duration::from_secs(1); + assert_eq!(8, *state.evaluate(now)); + assert!(state.consume_changed()); + + let now = now + Duration::from_secs(1); + assert_eq!(4, *state.evaluate(now)); + assert!(state.consume_changed()); + + state.insert(10, now + Duration::from_secs(1), 0); + state.insert(11, now, 1); + state.insert(12, now + Duration::from_secs(1), 1); + assert!(state.consume_changed()); + assert_eq!(12, *state); + + let now = now + Duration::from_secs(2); + assert_eq!(4, *state.evaluate(now)); + assert!(state.consume_changed()); + + state.insert(13, now + Duration::from_secs(1), 0); + state.insert(14, now, 1); + assert_eq!(14, *state); + let now = now + Duration::from_secs(2); + assert_eq!(4, *state.evaluate(now)); + assert!(state.consume_changed()); + } + + #[test] + fn changed() { + let _ = env_logger::builder() + .is_test(true) + .filter_level(LevelFilter::Trace) + .try_init(); + + let mut state = CommandedState::<_, 2>::new(4); + let now = Instant::now(); + assert!(!state.consume_changed()); + assert_eq!(4, *state); + + state.insert(4, now + Duration::from_secs(1), 0); + assert!(!state.consume_changed()); + + let now = Instant::now() + Duration::from_secs(2); + assert_eq!(4, *state.evaluate_changed(now)); + assert!(state.consume_changed()); + + state.insert_changed(4, now + Duration::from_secs(1), 0); + assert!(state.consume_changed()); + + let now = Instant::now() + Duration::from_secs(2); + assert_eq!(4, *state.evaluate(now)); + } +} diff --git a/flight/src/comms/command.rs b/flight/src/comms/command.rs new file mode 100644 index 0000000..38e1156 --- /dev/null +++ b/flight/src/comms/command.rs @@ -0,0 +1,42 @@ +use nautilus_common::command::valid_priority_command::ValidPriorityCommand; +use std::fmt::Debug; +use std::ops::{Deref, DerefMut}; +use std::time::Instant; + +#[derive(Clone, Debug)] +pub struct InstantPriorityCommand +where + T: Clone + Debug, +{ + pub inner: T, + pub valid_until: Instant, + pub priority: u8, +} + +impl From> for InstantPriorityCommand +where + T: Clone + Debug, +{ + fn from(value: ValidPriorityCommand) -> Self { + let valid_until = value.get_valid_until_instant(); + Self { + inner: value.inner, + valid_until, + priority: value.priority, + } + } +} + +impl Deref for InstantPriorityCommand { + type Target = T; + + fn deref(&self) -> &Self::Target { + &self.inner + } +} + +impl DerefMut for InstantPriorityCommand { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.inner + } +} diff --git a/flight/src/comms/mod.rs b/flight/src/comms/mod.rs index 18a8cf9..462449c 100644 --- a/flight/src/comms/mod.rs +++ b/flight/src/comms/mod.rs @@ -1,3 +1,5 @@ +pub mod command; + use crate::scheduler::{CyclicTask, TaskHandle}; use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector}; use anyhow::{Result, ensure}; @@ -54,7 +56,11 @@ impl<'a, A> CommsTask<'a, A> where A: ToSocketAddrs + Debug, { - pub fn new(local_port: u16, ground_address: A, state_vector: &'a StateVector) -> Result { + pub fn new<'b: 'a>( + local_port: u16, + ground_address: A, + state_vector: &'b StateVector, + ) -> Result { trace!( "CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})", type_name::() diff --git a/flight/src/hardware/channelization.rs b/flight/src/hardware/channelization.rs index a0174ab..f7950d5 100644 --- a/flight/src/hardware/channelization.rs +++ b/flight/src/hardware/channelization.rs @@ -46,12 +46,12 @@ pub enum PinoutChannel { ExtB(u8), } -pub struct DevicePin<'a, Device: PinDevice> { +pub struct DevicePin { pin: u8, - device: &'a Device, + device: Device, } -impl Pin for DevicePin<'_, Device> { +impl Pin for DevicePin { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { trace!( "ChannelPin::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})", @@ -61,12 +61,12 @@ impl Pin for DevicePin<'_, Device> { } } -pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> { - ExtA(DevicePin<'a, A>), - ExtB(DevicePin<'a, B>), +pub enum ChannelPin { + ExtA(DevicePin), + ExtB(DevicePin), } -impl Pin for ChannelPin<'_, A, B> { +impl Pin for ChannelPin { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { trace!( "ChannelPin::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})", @@ -81,11 +81,11 @@ impl Pin for ChannelPin<'_, A, B> { } impl PinoutChannel { - pub fn get_pin<'a>( - self, - ext_a: &'a (impl PinDevice + Debug), - ext_b: &'a (impl PinDevice + Debug), - ) -> impl Pin { + pub fn get_pin(self, ext_a: A, ext_b: B) -> ChannelPin + where + A: PinDevice + Debug, + B: PinDevice + Debug, + { trace!("PinoutChannel::get_pin(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}"); match self { PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }), diff --git a/flight/src/hardware/mcp23017/task.rs b/flight/src/hardware/mcp23017/task.rs index 1de3f29..82a9a85 100644 --- a/flight/src/hardware/mcp23017/task.rs +++ b/flight/src/hardware/mcp23017/task.rs @@ -1,9 +1,11 @@ +use crate::commanded_state::CommandedState; use crate::hardware::mcp23017::Mcp23017; use crate::hardware::pin::PinDevice; use crate::scheduler::{CyclicTask, TaskHandle}; use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector}; use embedded_hal::digital::PinState; use log::trace; +use std::array; use std::fmt::{Debug, Formatter}; use std::sync::mpsc::Receiver; use std::time::Instant; @@ -20,7 +22,9 @@ pub enum Mcp23017Message { 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:?})"); + trace!( + "TaskHandle::set_pin(self: {self:?}, pin: {pin}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})" + ); // This can only fail if the other end is disconnected - which we intentionally want to // ignore let _ = self.sender.send(Mcp23017Message::SetPin { @@ -68,107 +72,12 @@ impl AllPins { fn new() -> Self { trace!("AllPins::new()"); Self { - pins: [PinData::new(); _], + pins: array::repeat(PinData::new(PinState::Low)), } } } -#[derive(Copy, Clone, Debug)] -struct PinData { - state: PinState, - valid_until: Option, - priority: u8, - next_state: PinState, - next_validity: Option, - next_priority: u8, - default: PinState, - changed: bool, - value: PinState, -} - -impl PinData { - fn new() -> Self { - trace!("PinData::new()"); - Self { - state: PinState::Low, - valid_until: None, - priority: 0, - next_state: PinState::Low, - next_validity: None, - next_priority: 0, - default: PinState::Low, - changed: false, - value: PinState::Low, - } - } - - fn evaluate(&mut self, now: Instant) { - trace!("PinData::evaluate(self: {self:?}, now: {now:?})"); - // Do this twice to check both the current and the current next - // If the current is currently invalid, we'd upgrade the next to current - for _ in 0..2 { - let is_current_valid = self.valid_until.is_some_and(|current| current >= now); - if is_current_valid { - self.value = self.state; - return; - } - - if self.valid_until.is_some() { - self.changed = true; - } - self.state = self.next_state; - self.valid_until = self.next_validity; - self.priority = self.next_priority; - - self.next_validity = None; - self.next_priority = 0; - } - - self.value = self.default; - } - - fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { - trace!( - "PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})" - ); - let can_replace_current = self - .valid_until - .is_none_or(|current| current <= valid_until) - || self.priority == priority; - let can_replace_next = self.next_validity.is_none_or(|next| next <= valid_until) - || self.next_priority == priority; - - if priority >= self.priority { - // This is now the highest priority thing (or most recent of equal priority) - if can_replace_current { - if can_replace_next { - self.next_validity = None; - self.next_priority = 0; - } - } else { - self.next_state = self.state; - self.next_validity = self.valid_until; - self.next_priority = self.priority; - } - self.state = value; - self.valid_until = Some(valid_until); - self.priority = priority; - self.changed = true; - self.value = value; - } else { - // This is not the highest priority thing - if self.priority >= self.next_priority { - // Higher priority than the next highest though - self.next_state = value; - self.next_validity = Some(valid_until); - self.next_priority = priority; - self.changed = true; - } else { - // Not high enough priority to remember - } - } - } -} +type PinData = CommandedState; impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> { pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self { @@ -209,7 +118,7 @@ impl CyclicTask for Mcp23017Task<'_, M> { priority, } => { if (0u8..16u8).contains(&pin) { - self.pins.pins[pin as usize].set(value, valid_until, priority); + self.pins.pins[pin as usize].insert(value, valid_until, priority); } } } @@ -217,9 +126,8 @@ impl CyclicTask for Mcp23017Task<'_, M> { for pin in 0u8..16u8 { let current_pin = &mut self.pins.pins[pin as usize]; - let state = current_pin.value; - if current_pin.changed { - current_pin.changed = false; + let state = **current_pin; + if current_pin.consume_changed() { // This shouldn't be able to fail // TODO: handle error case let _ = self.mcp23017.set_pin(pin, state); @@ -229,7 +137,7 @@ impl CyclicTask for Mcp23017Task<'_, M> { if changed { let _ = self.mcp23017.flush(); self.state.update(|s| { - s.pins = self.pins.pins.map(|pin| pin.value == PinState::High); + s.pins = array::from_fn(|i| *self.pins.pins[i] == PinState::High); }); } } diff --git a/flight/src/hardware/pin.rs b/flight/src/hardware/pin.rs index 0949c2b..81532e9 100644 --- a/flight/src/hardware/pin.rs +++ b/flight/src/hardware/pin.rs @@ -1,5 +1,6 @@ use embedded_hal::digital::PinState; -use nautilus_common::command::{SetPin, ValidPriorityCommand}; +use nautilus_common::command::set_pin::SetPin; +use nautilus_common::command::valid_priority_command::ValidPriorityCommand; use std::time::Instant; pub trait PinDevice { diff --git a/flight/src/lib.rs b/flight/src/lib.rs index bb158d6..8119840 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -6,6 +6,7 @@ use crate::hardware::initialize; use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task}; use crate::hardware::mct8316a::Mct8316a; use crate::hardware::pin::PinDevice; +use crate::rcs::RcsTask; use crate::scheduler::Scheduler; use crate::state_vector::StateVector; use anyhow::Result; @@ -70,10 +71,13 @@ pub fn run() -> Result<()> { )?; let b_id = task_b.get_id(); + let rcs = s.run_cyclic("rcs-task", RcsTask::new(&task_a, &task_b), 10)?; + let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?; comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?; comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?; comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?; + comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?; let comms = s.run_cyclic("comms-task", comms, 10)?; let comms_id = *comms; @@ -122,6 +126,7 @@ pub fn run() -> Result<()> { Ok(()) } +mod commanded_state; mod comms; mod data; mod rcs; diff --git a/flight/src/rcs/mod.rs b/flight/src/rcs/mod.rs index 3cdfa9a..9c96693 100644 --- a/flight/src/rcs/mod.rs +++ b/flight/src/rcs/mod.rs @@ -1,13 +1,153 @@ -// use crate::hardware::mcp23017::Mcp23017OutputPin; -// -// struct RcsTask { -// pin: PIN, -// } -// -// impl RcsTask { -// pub fn new(pin: PIN) -> Self { -// Self { -// pin -// } -// } -// } +mod thruster_balancing; + +use crate::commanded_state::CommandedState; +use crate::comms::command::InstantPriorityCommand; +use crate::hardware::channelization::{ + ChannelPin, PinoutChannel, RCS0, RCS1, RCS2, RCS3, RCS4, RCS5, RCS6, RCS7, RCS8, RCS9, +}; +use crate::hardware::pin::{Pin, PinDevice}; +use crate::rcs::thruster_balancing::BalancingConstants; +use crate::scheduler::{CyclicTask, TaskHandle}; +use embedded_hal::digital::PinState; +use log::trace; +use nautilus_common::command::set_rcs::SetRcs; +use nautilus_common::command::valid_priority_command::ValidPriorityCommand; +use nautilus_common::math::Vector; +use std::fmt::Debug; +use std::iter::zip; +use std::sync::mpsc::Receiver; +use std::time::{Duration, Instant}; + +const PIN_PRIORITY: u8 = 0; + +struct Thruster { + channel: PinoutChannel, + position: Vector, + direction: Vector, + force: f64, +} + +impl Thruster { + const fn new(channel: PinoutChannel, position: Vector, direction: Vector, force: f64) -> Self { + Self { + channel, + position, + direction, + force, + } + } +} + +const RCS_THRUSTERS: [Thruster; 10] = [ + Thruster::new(RCS0, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS1, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS2, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS3, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS4, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS5, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS6, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS7, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS8, Vector::ZERO, Vector::ZERO, 1.0), + Thruster::new(RCS9, Vector::ZERO, Vector::ZERO, 1.0), +]; + +#[derive(Clone, Debug)] +pub enum RcsMessage { + SetRcs(InstantPriorityCommand), +} + +impl TaskHandle { + pub fn set_rcs(&self, cmd: InstantPriorityCommand) { + trace!("TaskHandle::set_rcs(self: {self:?}, cmd: {cmd:?})"); + // This can only fail if the other end is disconnected - which we intentionally want to + // ignore + let _ = self.sender.send(RcsMessage::SetRcs(cmd)); + } + + pub fn new_set_rcs_callback<'a>(&self) -> impl Fn(ValidPriorityCommand) + 'a + where + Self: Sized + Clone + 'a, + { + let this = self.clone(); + move |cmd| { + this.set_rcs(cmd.into()); + } + } +} + +#[derive(Clone)] +pub struct RcsTask

{ + rcs_pins: [P; RCS_THRUSTERS.len()], + command: CommandedState<(Vector, Vector)>, +} + +impl RcsTask> +where + A: PinDevice + Debug + Clone, + B: PinDevice + Debug + Clone, +{ + pub fn new(a: &A, b: &B) -> Self { + let rcs_pins = RCS_THRUSTERS.map(|thruster| thruster.channel.get_pin(a.clone(), b.clone())); + Self { + rcs_pins, + command: CommandedState::new((Vector::ZERO, Vector::ZERO)), + } + } +} + +impl

CyclicTask for RcsTask

+where + P: Pin, +{ + type Message = RcsMessage; + type Data = (); + + fn get_data(&self) -> Self::Data { + trace!("RcsTask::get_data(self)"); + } + + fn step(&mut self, receiver: &Receiver, step_time: Instant) { + trace!("RcsTask::step(self, receiver, step_time: {step_time:?})"); + + self.command.evaluate(step_time); + + while let Ok(msg) = receiver.try_recv() { + match msg { + RcsMessage::SetRcs(InstantPriorityCommand { + inner: + SetRcs { + translation, + rotation, + }, + valid_until, + priority, + }) => { + self.command + .insert((translation, rotation), valid_until, priority); + } + } + } + + let (translation, rotation) = *self.command; + + let throttle = thruster_balancing::balance_thrust( + translation, + rotation, + &RCS_THRUSTERS, + BalancingConstants::default(), + ); + + for (pin, throttle) in zip(&mut self.rcs_pins, throttle) { + pin.set( + if throttle >= 0.5 { + PinState::High + } else { + PinState::Low + }, + // This signal is valid for the next half second + step_time + Duration::from_secs(1), + PIN_PRIORITY, + ); + } + } +} diff --git a/flight/src/rcs/thruster_balancing.rs b/flight/src/rcs/thruster_balancing.rs new file mode 100644 index 0000000..cbf3f1a --- /dev/null +++ b/flight/src/rcs/thruster_balancing.rs @@ -0,0 +1,302 @@ +use crate::rcs::Thruster; +use good_lp::{ + Expression, IntoAffineExpression, ProblemVariables, Solution, SolverModel, microlp, variable, +}; +use log::error; +use nautilus_common::math::Vector; +use std::ops::Add; + +#[derive(Copy, Clone, Debug)] +pub(super) struct BalancingConstants { + /// Should be smaller than the other two constants + throttle: f64, + force: f64, + torque: f64, +} + +impl Default for BalancingConstants { + fn default() -> Self { + Self { + throttle: 1e-3, + force: 1.0, + torque: 1.0, + } + } +} + +#[allow(clippy::similar_names)] +pub(super) fn balance_thrust( + desired_force: Vector, + desired_torque: Vector, + thrusters: &[Thruster; N], + constants: BalancingConstants, +) -> [f64; N] { + let mut problem = ProblemVariables::new(); + let throttles: [_; N] = std::array::from_fn(|_| problem.add(variable().min(0).max(1))); + + let force_error_x_1 = problem.add(variable().min(0)); + let force_error_x_2 = problem.add(variable().min(0)); + let force_error_x = force_error_x_1 + force_error_x_2; + let force_delta_x = force_error_x_1 - force_error_x_2; + let force_error_y_1 = problem.add(variable().min(0)); + let force_error_y_2 = problem.add(variable().min(0)); + let force_error_y = force_error_y_1 + force_error_y_2; + let force_delta_y = force_error_y_1 - force_error_y_2; + let force_error_z_1 = problem.add(variable().min(0)); + let force_error_z_2 = problem.add(variable().min(0)); + let force_error_z = force_error_z_1 + force_error_z_2; + let force_delta_z = force_error_z_1 - force_error_z_2; + let force_error = force_error_x + force_error_y + force_error_z; + + let torque_error_x_1 = problem.add(variable().min(0)); + let torque_error_x_2 = problem.add(variable().min(0)); + let torque_error_x = torque_error_x_1 + torque_error_x_2; + let torque_delta_x = torque_error_x_1 - torque_error_x_2; + let torque_error_y_1 = problem.add(variable().min(0)); + let torque_error_y_2 = problem.add(variable().min(0)); + let torque_error_y = torque_error_y_1 + torque_error_y_2; + let torque_delta_y = torque_error_y_1 - torque_error_y_2; + let torque_error_z_1 = problem.add(variable().min(0)); + let torque_error_z_2 = problem.add(variable().min(0)); + let torque_error_z = torque_error_z_1 + torque_error_z_2; + let torque_delta_z = torque_error_z_1 - torque_error_z_2; + let torque_error = torque_error_x + torque_error_y + torque_error_z; + + let total_throttle = throttles.iter().fold(0.into_expression(), Expression::add); + + let generated_force_x = (0..N) + .map(|i| thrusters[i].direction.x * thrusters[i].force * throttles[i]) + .fold(0.into_expression(), Expression::add); + let generated_force_y = (0..N) + .map(|i| thrusters[i].direction.y * thrusters[i].force * throttles[i]) + .fold(0.into_expression(), Expression::add); + let generated_force_z = (0..N) + .map(|i| thrusters[i].direction.z * thrusters[i].force * throttles[i]) + .fold(0.into_expression(), Expression::add); + + let torque_directions: [_; N] = std::array::from_fn(|i| { + let cross = thrusters[i].position.cross_product(thrusters[i].direction); + if cross == Vector::ZERO { + Vector::ZERO + } else { + cross.normalize() + } + }); + let moment_arms: [f64; N] = std::array::from_fn(|i| thrusters[i].position.length()); + + let generated_torque_x = (0..N) + .map(|i| torque_directions[i].x * thrusters[i].force * throttles[i] * moment_arms[i]) + .fold(0.into_expression(), Expression::add); + let generated_torque_y = (0..N) + .map(|i| torque_directions[i].y * thrusters[i].force * throttles[i] * moment_arms[i]) + .fold(0.into_expression(), Expression::add); + let generated_torque_z = (0..N) + .map(|i| torque_directions[i].z * thrusters[i].force * throttles[i] * moment_arms[i]) + .fold(0.into_expression(), Expression::add); + + let to_minimize = (total_throttle.clone() * constants.throttle) + + (force_error.clone() * constants.force) + + (torque_error.clone() * constants.torque); + + let solution = problem + .minimise(to_minimize) + .using(microlp) + .with(force_delta_x.eq(generated_force_x - desired_force.x)) + .with(force_delta_y.eq(generated_force_y - desired_force.y)) + .with(force_delta_z.eq(generated_force_z - desired_force.z)) + .with(torque_delta_x.eq(generated_torque_x - desired_torque.x)) + .with(torque_delta_y.eq(generated_torque_y - desired_torque.y)) + .with(torque_delta_z.eq(generated_torque_z - desired_torque.z)) + .solve(); + + match solution { + Ok(solution) => std::array::from_fn(|i| solution.value(throttles[i])), + Err(err) => { + error!("Failed to balance thrusters {err}"); + std::array::repeat(0.0) + } + } +} + +#[cfg(test)] +mod tests { + use crate::hardware::channelization::PinoutChannel; + use crate::rcs::Thruster; + use crate::rcs::thruster_balancing::{BalancingConstants, balance_thrust}; + use nautilus_common::math::Vector; + + const EPSILON: f64 = 1e-6; + + #[test] + fn test_thruster_balancing() { + // We can't fire the one thruster we have because it will induce a torque + assert_eq!( + [0.0], + balance_thrust( + Vector::X, + Vector::ZERO, + &[Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + Vector::X, + 1.0 + ),], + BalancingConstants::default(), + ) + ); + + // A single perfectly aligned thruster + assert_eq!( + [1.0], + balance_thrust( + Vector::X, + Vector::ZERO, + &[Thruster::new( + PinoutChannel::ExtA(0), + Vector::ZERO, + Vector::X, + 1.0 + ),], + BalancingConstants::default(), + ) + ); + + // Two balanced thrusters should both be fired + assert_eq!( + [1.0, 1.0], + balance_thrust( + Vector::X, + Vector::ZERO, + &[ + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + Vector::X, + 0.5 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, -1.0, 0.0), + Vector::X, + 0.5 + ), + ], + BalancingConstants::default(), + ) + ); + + // Two thrusters with different moment arms + // Despite desiring a lot of force, we can't generate too much without inducing a torque + assert!( + balance_thrust( + Vector::X * 10.0, + Vector::ZERO, + &[ + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + Vector::X, + 0.5 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, -2.0, 0.0), + Vector::X, + 0.5 + ), + ], + BalancingConstants::default(), + ) + .into_iter() + .zip([1.0, 0.5]) + .map(|(a, b)| a - b) + .sum::() + .abs() + < EPSILON + ); + + // Four perfectly balanced thrusters + assert!( + balance_thrust( + Vector::ZERO, + Vector::Z * 2.0, + &[ + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + Vector::X, + 1.0 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + -Vector::X, + 1.0 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, -1.0, 0.0), + Vector::X, + 1.0 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, -1.0, 0.0), + -Vector::X, + 1.0 + ), + ], + BalancingConstants::default(), + ) + .into_iter() + .zip([0.0, 1.0, 1.0, 0.0]) + .map(|(a, b)| a - b) + .sum::() + .abs() + < EPSILON + ); + + // Four perfectly balanced thrusters + assert!( + balance_thrust( + Vector::X * 2.0, + Vector::Z * 2.0, + &[ + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + Vector::X, + 1.0 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, 1.0, 0.0), + -Vector::X, + 1.0 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, -1.0, 0.0), + Vector::X, + 1.0 + ), + Thruster::new( + PinoutChannel::ExtA(0), + Vector::new(0.0, -1.0, 0.0), + -Vector::X, + 1.0 + ), + ], + BalancingConstants::default(), + ) + .into_iter() + // Only the one thruster should fire - we can't do any better than that + // any more thrusters we fire won't give us any more thrust + .zip([0.0, 0.0, 1.0, 0.0]) + .map(|(a, b)| a - b) + .sum::() + .abs() + < EPSILON + ); + } +} diff --git a/flight/src/scheduler/mod.rs b/flight/src/scheduler/mod.rs index 12386af..b3c687d 100644 --- a/flight/src/scheduler/mod.rs +++ b/flight/src/scheduler/mod.rs @@ -13,8 +13,6 @@ use std::time::{Duration, Instant}; #[derive(Clone, Debug)] pub struct TaskHandle { - #[allow(dead_code)] - pub name: String, pub sender: Sender, data: Data, } @@ -46,7 +44,7 @@ pub trait CyclicTask { impl CyclicTask for F where - F: Fn(), + F: FnMut(), { type Message = (); type Data = (); @@ -58,7 +56,10 @@ where } } -pub struct Scheduler<'s, 'e> { +pub struct Scheduler<'s, 'e> +where + 'e: 's, +{ scope: &'s Scope<'s, 'e>, running: Arc, } @@ -68,7 +69,7 @@ impl<'s> Scheduler<'s, '_> { where F: FnOnce(Scheduler<'_, 'env>) -> R, { - trace!("Scheduler::new(running: {running:?}, f)"); + trace!("Scheduler::scope(running: {running:?}, f)"); thread::scope(|scope: &Scope| { let running_result = running.clone(); // This will automatically set running to false when it drops @@ -83,16 +84,29 @@ impl<'s> Scheduler<'s, '_> { }) } + // pub fn inner_scope<'env, F, R>(&self, f: F) -> R + // where + // F: FnOnce(Scheduler<'_, 'env>) -> R, + // 's: 'env, + // { + // trace!("Scheduler::inner_scope(self)"); + // thread::scope(|scope: &Scope<'_, 'env>| { + // f(Scheduler { + // scope, + // running: self.running.clone(), + // }) + // }) + // } + #[allow(dead_code)] - pub fn run<'t, T>( + pub fn run( &self, name: impl Into, task: T, ) -> Result> where - T: Task + Send + Debug + 't, + T: Task + Send + Debug + 's, T::Message: Send, - 't: 's, { let name = name.into(); trace!( @@ -107,19 +121,18 @@ impl<'s> Scheduler<'s, '_> { .spawn_scoped(self.scope, move || { task.run(receiver, running); })?; - Ok(TaskHandle { name, sender, data }) + Ok(TaskHandle { sender, data }) } - pub fn run_cyclic<'t, T>( + pub fn run_cyclic( &self, name: impl Into, mut task: T, frequency: u64, ) -> Result> where - T: CyclicTask + Send + 't, + T: CyclicTask + Send + 's, T::Message: Send, - 't: 's, { let name = name.into(); trace!( @@ -144,6 +157,6 @@ impl<'s> Scheduler<'s, '_> { } } })?; - Ok(TaskHandle { name, sender, data }) + Ok(TaskHandle { sender, data }) } } diff --git a/ground/src/command.rs b/ground/src/command.rs index d9edad2..9e34001 100644 --- a/ground/src/command.rs +++ b/ground/src/command.rs @@ -2,9 +2,13 @@ use anyhow::bail; use api::client::Client; use api::client::command::CommandRegistry; use api::macros::IntoCommandDefinition; -use chrono::{DateTime, Utc}; +use chrono::{DateTime, TimeDelta, Utc}; use log::{error, trace}; -use nautilus_common::command::{Command, OwnedCommandHeader, SetPin, ValidPriorityCommand}; +use nautilus_common::command::set_pin::SetPin; +use nautilus_common::command::set_rcs::SetRcs; +use nautilus_common::command::valid_priority_command::ValidPriorityCommand; +use nautilus_common::command::{Command, OwnedCommandHeader}; +use nautilus_common::math::Vector; use nautilus_common::udp::tokio::AsyncUdpSocketExt; use std::fmt::Debug; use std::net::SocketAddr; @@ -29,6 +33,32 @@ struct SetPinCommand { index: u8, state: bool, } +impl From for SetPin { + fn from(value: SetPinCommand) -> Self { + Self { + pin: value.index, + value: value.state, + } + } +} + +#[derive(IntoCommandDefinition)] +struct RcsCommand { + translate_x: f64, + translate_y: f64, + translate_z: f64, + rotate_x: f64, + rotate_y: f64, + rotate_z: f64, +} +impl From for SetRcs { + fn from(value: RcsCommand) -> Self { + Self { + translation: Vector::new(value.translate_x, value.translate_y, value.translate_z), + rotation: Vector::new(value.rotate_x, value.rotate_y, value.rotate_z), + } + } +} impl<'a> CommandHandler<'a> { pub fn new( @@ -69,11 +99,8 @@ impl<'a> CommandHandler<'a> { outgoing_commands_tx.try_send_command( &command_name, &ValidPriorityCommand { - inner: SetPin { - pin: cmd.index, - value: cmd.state, - }, - valid_until: MAX_DATETIME, // header.timestamp + TimeDelta::seconds(5), + inner: SetPin::from(cmd), + valid_until: MAX_DATETIME, priority: 0, } )?; @@ -84,13 +111,37 @@ impl<'a> CommandHandler<'a> { }) .collect::>(); - commands.push(self.cmd.register_handler("shutdown", move |_, ()| -> anyhow::Result<_> { - trace!("Shutting Down Flight"); + { + let outgoing_commands_tx = outgoing_commands_tx.clone(); + commands.push(self.cmd.register_handler("shutdown", move |_, ()| -> anyhow::Result<_> { + trace!("Shutting Down Flight"); - outgoing_commands_tx.try_send_command("/shutdown", &())?; + outgoing_commands_tx.try_send_command("/shutdown", &())?; - Ok("Command Executed Successfully".to_string()) - })); + Ok("Command Executed Successfully".to_string()) + })); + } + + { + let outgoing_commands_tx = outgoing_commands_tx.clone(); + commands.push(self.cmd.register_handler("rcs.set", move |header, cmd: RcsCommand| -> anyhow::Result<_> { + trace!("Sending Rcs Set Command"); + + outgoing_commands_tx.try_send_command( + "/rcs/set", + &ValidPriorityCommand { + inner: SetRcs::from(cmd), + valid_until: header.timestamp + TimeDelta::seconds(5), + priority: 0, + } + )?; + + Ok("Command Executed Successfully".to_string()) + })); + } + + // We no longer need this + drop(outgoing_commands_tx); let mut buffer = [0u8; 512]; while !self.cancel.is_cancelled() { diff --git a/ground/src/telemetry.rs b/ground/src/telemetry.rs index a24d516..46ce91b 100644 --- a/ground/src/telemetry.rs +++ b/ground/src/telemetry.rs @@ -1,7 +1,7 @@ use api::client::Client; use api::client::telemetry::{TelemetryHandle, TelemetryRegistry}; -use futures::TryFutureExt; -use futures::future::join_all; +use futures::future::{Either, join_all, select}; +use futures::{TryFutureExt, pin_mut}; use log::{error, trace}; use nautilus_common::on_drop::on_drop; use nautilus_common::telemetry::{CommsState, SwitchBank, Telemetry, TelemetryMessage}; @@ -12,7 +12,7 @@ use std::sync::Arc; use tokio::net::UdpSocket; use tokio::sync::{RwLock, RwLockWriteGuard}; use tokio::task::yield_now; -use tokio::{join, pin, select, try_join}; +use tokio::{join, select, try_join}; use tokio_util::sync::CancellationToken; pub struct TelemetryHandler<'a> { @@ -52,7 +52,18 @@ impl<'a> TelemetryHandler<'a> { .build()?; runtime.block_on(async { - let mut context = TelemetryContext::new(&self).await; + // Allow cancellation while waiting for this to initialize + let mut context = { + let context = TelemetryContext::new(&self); + pin_mut!(context); + let cancellation_future = self.cancel.cancelled(); + pin_mut!(cancellation_future); + let init_or_cancel = select(context, cancellation_future).await; + match init_or_cancel { + Either::Left((success, _)) => success, + Either::Right(_) => return Ok(()), + } + }; let mut buffer = [0u8; 512]; @@ -75,7 +86,7 @@ impl<'a> TelemetryHandler<'a> { // Update the value in the lock *lock = addr; }; - pin!(flight_addr_update); + tokio::pin!(flight_addr_update); // We can do these two operations concurrently join!(