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

Compare commits

base: sergeysav:086ac7f1958f592bacd4a3da4ffd5fdf525fedcf
Branches Tags
sergeysav:main
...
compare: sergeysav:main
Branches Tags
sergeysav:main

10 Commits
086ac7f195 ... main

Author SHA1 Message Date
Sergey Savelyev
59b5679dda rename pin command handler factory method 2025-11-30 09:09:31 -08:00
Sergey Savelyev
ea56b9865e adds command registration 2025-11-30 08:50:35 -08:00
Sergey Savelyev
d53d78434c increase command flexibility 2025-11-30 08:02:39 -08:00
Sergey Savelyev
eefc3293b4 lint 2025-10-26 09:56:56 -07:00
Sergey Savelyev
5455935f3a cleanup 2025-10-26 08:56:59 -07:00
Sergey Savelyev
e0f17649b2 initial state vector implementation 2025-10-25 16:11:39 -07:00
Sergey Savelyev
b067ae5cec initial comms 2025-10-25 12:23:23 -07:00
Sergey Savelyev
fd63bdc0c9 initial safety biased leased control architecture 2025-10-22 22:20:28 -07:00
Sergey Savelyev
26271fcb17 output channelization 2025-10-20 21:02:57 -07:00
Sergey Savelyev
2bcb122319 adds simulated hardware 2025-10-20 20:17:50 -07:00
49 changed files with 2358 additions and 877 deletions
Expand all files Collapse all files

322
Cargo.lock generated
View File

@@ -17,15 +17,6 @@ version = "1.0.100"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a23eb6b1614318a8071c9b2521f36b424b2c83db5eb3a0fead4a6c0809af6e61" 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]] [[package]]
name = "autocfg" name = "autocfg"
version = "1.4.0" version = "1.4.0"
@@ -44,12 +35,6 @@ version = "3.17.0"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf" checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf"
[[package]]
name = "bytemuck"
version = "1.23.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "9134a6ef01ce4b366b50689c94f82c14bc72bc5d0386829828a2e2752ef7958c"
[[package]] [[package]]
name = "cc" name = "cc"
version = "1.2.17" version = "1.2.17"
@@ -80,10 +65,20 @@ dependencies = [
"iana-time-zone", "iana-time-zone",
"js-sys", "js-sys",
"num-traits", "num-traits",
"serde",
"wasm-bindgen", "wasm-bindgen",
"windows-link", "windows-link",
] ]
[[package]]
name = "cobs"
version = "0.3.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "0fa961b519f0b462e3a3b4a34b64d119eeaca1d59af726fe450bbba07a9fc0a1"
dependencies = [
"thiserror",
]
[[package]] [[package]]
name = "colored" name = "colored"
version = "2.2.0" version = "2.2.0"
@@ -188,6 +183,18 @@ dependencies = [
"nb 1.1.0", "nb 1.1.0",
] ]
[[package]]
name = "embedded-io"
version = "0.4.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced"
[[package]]
name = "embedded-io"
version = "0.6.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d"
[[package]] [[package]]
name = "embedded-time" name = "embedded-time"
version = "0.12.1" version = "0.12.1"
@@ -207,102 +214,6 @@ dependencies = [
"log", "log",
] ]
[[package]]
name = "glam"
version = "0.14.0"
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"
[[package]] [[package]]
name = "hex" name = "hex"
version = "0.4.3" version = "0.4.3"
@@ -361,62 +272,19 @@ version = "0.4.28"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432" 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]] [[package]]
name = "nautilus_common" name = "nautilus_common"
version = "0.1.0" version = "0.1.0"
dependencies = [
"anyhow",
"chrono",
"ctrlc",
"fern",
"log",
"postcard",
"serde",
"thiserror",
]
[[package]] [[package]]
name = "nautilus_flight" name = "nautilus_flight"
@@ -425,22 +293,28 @@ dependencies = [
"anyhow", "anyhow",
"chrono", "chrono",
"crc", "crc",
"ctrlc",
"embedded-hal 1.0.0", "embedded-hal 1.0.0",
"embedded-hal-bus", "embedded-hal-bus",
"embedded-hal-mock", "embedded-hal-mock",
"fern",
"hex", "hex",
"log", "log",
"nalgebra", "nautilus_common",
"num-traits", "postcard",
"rpi-pal", "rpi-pal",
"thiserror",
] ]
[[package]] [[package]]
name = "nautilus_ground" name = "nautilus_ground"
version = "0.1.0" version = "0.1.0"
dependencies = [
"anyhow",
"chrono",
"hex",
"log",
"nautilus_common",
"postcard",
"serde",
]
[[package]] [[package]]
name = "nb" name = "nb"
@@ -475,20 +349,10 @@ version = "0.3.1"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
dependencies = [ dependencies = [
"num-complex 0.3.1", "num-complex",
"num-integer", "num-integer",
"num-iter", "num-iter",
"num-rational 0.3.2", "num-rational",
"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-traits", "num-traits",
] ]
@@ -501,15 +365,6 @@ dependencies = [
"num-traits", "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]] [[package]]
name = "num-integer" name = "num-integer"
version = "0.1.46" version = "0.1.46"
@@ -541,17 +396,6 @@ dependencies = [
"num-traits", "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]] [[package]]
name = "num-traits" name = "num-traits"
version = "0.2.19" version = "0.2.19"
@@ -568,10 +412,16 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d"
[[package]] [[package]]
name = "paste" name = "postcard"
version = "1.0.15" version = "1.1.3"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" checksum = "6764c3b5dd454e283a30e6dfe78e9b31096d9e32036b5d1eaac7a6119ccb9a24"
dependencies = [
"cobs",
"embedded-io 0.4.0",
"embedded-io 0.6.1",
"serde",
]
[[package]] [[package]]
name = "proc-macro2" name = "proc-macro2"
@@ -591,12 +441,6 @@ dependencies = [
"proc-macro2", "proc-macro2",
] ]
[[package]]
name = "rawpointer"
version = "0.2.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
[[package]] [[package]]
name = "rpi-pal" name = "rpi-pal"
version = "0.22.2" version = "0.22.2"
@@ -619,12 +463,33 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2" checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2"
[[package]] [[package]]
name = "safe_arch" name = "serde"
version = "0.7.4" version = "1.0.228"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323" checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e"
dependencies = [ 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]] [[package]]
@@ -633,19 +498,6 @@ version = "1.3.0"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" 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]] [[package]]
name = "spin_sleep" name = "spin_sleep"
version = "1.3.1" version = "1.3.1"
@@ -686,12 +538,6 @@ dependencies = [
"syn", "syn",
] ]
[[package]]
name = "typenum"
version = "1.18.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f"
[[package]] [[package]]
name = "unicode-ident" name = "unicode-ident"
version = "1.0.18" version = "1.0.18"
@@ -762,16 +608,6 @@ dependencies = [
"unicode-ident", "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]] [[package]]
name = "windows-core" name = "windows-core"
version = "0.52.0" version = "0.52.0"

12
Cargo.toml
View File

@@ -1,3 +1,13 @@
[workspace] [workspace]
resolver = "3" resolver = "3"
members = ["common", "flight", "ground"] members = ["common", "ground", "flight"]
[workspace.dependencies]
anyhow = "1.0.100"
fern = { version = "0.7.1", features = ["colored"] }
log = { version = "0.4.28" }
chrono = { version = "0.4.42", features = ["serde"] }
ctrlc = "3.5.0"
serde = { version = "1.0.228", features = ["derive"], default-features = false }
postcard = { version = "1.1.3", default-features = false, features = ["alloc"] }
thiserror = "2.0.17"

8
common/Cargo.toml
View File

@@ -4,3 +4,11 @@ version = "0.1.0"
edition = "2024" edition = "2024"
[dependencies] [dependencies]
anyhow = { workspace = true }
fern = { workspace = true }
log = { workspace = true }
chrono = { workspace = true }
ctrlc = { workspace = true }
serde = { workspace = true }
postcard = { workspace = true }
thiserror = { workspace = true }

78
common/src/command/mod.rs Normal file
View File

@@ -0,0 +1,78 @@
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<T>
where
T: Clone + Debug,
{
pub inner: T,
#[serde(with = "ts_nanoseconds")]
pub valid_until: DateTime<Utc>,
pub priority: u8,
}
impl<T> ValidPriorityCommand<T>
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)
}
}
}
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct CommandHeader<'a> {
pub name: &'a str,
pub data: &'a [u8],
}
pub trait Command: Serialize + DeserializeOwned {}
impl Command for () {}
impl Command for SetPin {}
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> Command
for ValidPriorityCommand<T>
{
}
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> Deref for ValidPriorityCommand<T> {
type Target = T;
fn deref(&self) -> &Self::Target {
&self.inner
}
}
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> DerefMut
for ValidPriorityCommand<T>
{
fn deref_mut(&mut self) -> &mut Self::Target {
&mut self.inner
}
}

29
common/src/lib.rs
View File

@@ -1,14 +1,21 @@
pub fn add(left: u64, right: u64) -> u64 { #![warn(clippy::all, clippy::pedantic)]
left + right use log::info;
} use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
#[cfg(test)] pub mod command;
mod tests { pub mod logger;
use super::*; pub mod telemetry;
pub mod udp;
#[test] /// Add a ctrl-c handler which will set an atomic flag to `false` when ctrl-c is detected
fn it_works() { ///
let result = add(2, 2); /// # Errors
assert_eq!(result, 4); /// If a system error occurred while trying to set the ctrl-c handler
} pub fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> anyhow::Result<()> {
ctrlc::set_handler(move || {
info!("Shutdown Requested");
flag.store(false, Ordering::Relaxed);
})?;
Ok(())
} }

12
flight/src/logger.rs → common/src/logger.rs
View File

@@ -5,13 +5,17 @@ use std::fs::create_dir_all;
use std::str::FromStr; use std::str::FromStr;
use std::{env, thread}; use std::{env, thread};
pub fn setup_logger() -> Result<()> { /// Set up the logger with a given package name
///
/// # Errors
/// If an error occurred while trying to set up the logger
pub fn setup_logger(package_name: &'static str) -> Result<()> {
let log_file = env::var("LOG_FILE").or_else(|_| { let log_file = env::var("LOG_FILE").or_else(|_| {
create_dir_all("logs/")?; create_dir_all("logs/")?;
anyhow::Ok(format!( anyhow::Ok(format!(
"logs/{}_{}.log", "logs/{}_{}.log",
env!("CARGO_PKG_NAME"), package_name,
chrono::Local::now().format("%Y%m%d_%H%M%S") chrono::Local::now().format("%Y%m%d_%H%M%S")
)) ))
})?; })?;
@@ -37,7 +41,7 @@ pub fn setup_logger() -> Result<()> {
level = colors.color(record.level()), level = colors.color(record.level()),
time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"), time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
target = record.target(), target = record.target(),
)) ));
}) })
.chain( .chain(
fern::Dispatch::new() fern::Dispatch::new()
@@ -47,6 +51,6 @@ pub fn setup_logger() -> Result<()> {
.chain(fern::log_file(log_file.clone())?) .chain(fern::log_file(log_file.clone())?)
.apply()?; .apply()?;
debug!("Logging to {} at level {}", log_file, log_level); debug!("Logging to {log_file} at level {log_level}");
Ok(()) Ok(())
} }

24
common/src/telemetry/mod.rs Normal file
View File

@@ -0,0 +1,24 @@
use chrono::serde::ts_nanoseconds;
use chrono::{DateTime, Utc};
use serde::{Deserialize, Serialize};
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct Telemetry {
#[serde(with = "ts_nanoseconds")]
pub timestamp: DateTime<Utc>,
pub message: TelemetryMessage,
}
#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
pub enum SwitchBank {
A,
B,
}
#[derive(Clone, Debug, Serialize, Deserialize)]
pub enum TelemetryMessage {
SwitchState {
bank: SwitchBank,
switches: [bool; 16],
},
}

140
common/src/udp.rs Normal file
View File

@@ -0,0 +1,140 @@
use crate::command::{Command, CommandHeader};
use crate::udp::UdpRecvPostcardError::ExtraData;
use crate::udp::UdpSendPostcardError::LengthMismatch;
use log::error;
use serde::{Deserialize, Serialize};
use std::io::ErrorKind;
use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
use thiserror::Error;
#[derive(Error, Debug)]
pub enum UdpRecvPostcardError {
#[error("IO Error")]
Io(#[from] std::io::Error),
#[error("Deserialization Error")]
Deserialization(#[from] postcard::Error),
#[error("Extra Data")]
ExtraData { amount: usize },
#[error("No Data")]
NoData,
}
#[derive(Error, Debug)]
pub enum UdpSendPostcardError {
#[error("IO Error")]
Io(#[from] std::io::Error),
#[error("Serialization Error")]
Serialization(#[from] postcard::Error),
#[error("Length Mismatch")]
LengthMismatch { expected: usize, actual: usize },
}
pub trait UdpSocketExt {
/// Receive a CBOR encoded message from this UDP Socket
///
/// # Errors
/// An error that could have occurred while trying to receive this message.
/// If no data was received a `UdpRecvCborError::NoData` error would be returned.
fn recv_postcard<'de, T: Deserialize<'de>>(
&self,
buffer: &'de mut [u8],
) -> Result<(T, SocketAddr), UdpRecvPostcardError>;
/// Send a CBOR encoded message to an address using this socket
///
/// # Errors
/// An error that could have occurred while trying to send this message
fn send_postcard<T: Serialize + ?Sized, A: ToSocketAddrs>(
&self,
data: &T,
buffer: &mut [u8],
addr: A,
) -> Result<(), UdpSendPostcardError>;
/// Send a command message to an address using this socket
///
/// # Errors
/// An error that could have occurred while trying to send this message
fn send_command<T: Command, A: ToSocketAddrs>(
&self,
name: &str,
data: &T,
addr: A,
) -> Result<(), UdpSendPostcardError>;
}
impl UdpSocketExt for UdpSocket {
fn recv_postcard<'de, T: Deserialize<'de>>(
&self,
buffer: &'de mut [u8],
) -> Result<(T, SocketAddr), UdpRecvPostcardError> {
match self.recv_from(buffer) {
Ok((size, addr)) => match postcard::take_from_bytes::<T>(&buffer[..size]) {
Ok((res, rem)) => {
if !rem.is_empty() {
return Err(ExtraData { amount: rem.len() });
}
Ok((res, addr))
}
Err(err) => Err(err.into()),
},
Err(err) => match err.kind() {
ErrorKind::WouldBlock | ErrorKind::TimedOut => Err(UdpRecvPostcardError::NoData),
_ => Err(err.into()),
},
}
}
fn send_postcard<T: Serialize + ?Sized, A: ToSocketAddrs>(
&self,
data: &T,
buffer: &mut [u8],
addr: A,
) -> Result<(), UdpSendPostcardError> {
match postcard::to_slice(data, buffer) {
Ok(result) => {
let size_encoded = result.len();
match self.send_to(result, addr) {
Ok(size_sent) => {
if size_encoded != size_sent {
return Err(LengthMismatch {
expected: size_encoded,
actual: size_sent,
});
}
Ok(())
}
Err(e) => Err(e.into()),
}
}
Err(e) => Err(e.into()),
}
}
fn send_command<T: Command, A: ToSocketAddrs>(
&self,
name: &str,
data: &T,
addr: A,
) -> Result<(), UdpSendPostcardError> {
let mut inner_buffer = [0u8; 512];
let inner_buffer = postcard::to_slice(data, &mut inner_buffer)?;
let mut buffer = [0u8; 512];
let buffer = postcard::to_slice(
&CommandHeader {
name,
data: inner_buffer,
},
&mut buffer,
)?;
let size_encoded = buffer.len();
let size_sent = self.send_to(buffer, addr)?;
if size_encoded != size_sent {
return Err(LengthMismatch {
expected: size_encoded,
actual: size_sent,
});
}
Ok(())
}
}

13
flight/Cargo.toml
View File

@@ -4,20 +4,17 @@ version = "0.0.1"
edition = "2024" edition = "2024"
[dependencies] [dependencies]
anyhow = "1.0.100" anyhow = { workspace = true }
fern = { version = "0.7.1", features = ["colored"] } log = { workspace = true, features = ["max_level_trace", "release_max_level_debug"] }
log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] } chrono = { workspace = true }
chrono = "0.4.42"
embedded-hal = "1.0.0" embedded-hal = "1.0.0"
embedded-hal-bus = { version = "0.3.0", features = ["std"] } embedded-hal-bus = { version = "0.3.0", features = ["std"] }
embedded-hal-mock = { version = "0.11.1", optional = true } embedded-hal-mock = { version = "0.11.1", optional = true }
rpi-pal = { version = "0.22.2", features = ["hal"], optional = true } rpi-pal = { version = "0.22.2", features = ["hal"], optional = true }
nalgebra = "0.34.1"
hex = "0.4.3" hex = "0.4.3"
thiserror = "2.0.17"
num-traits = "0.2.19"
crc = "3.3.0" crc = "3.3.0"
ctrlc = { version = "3.5" } nautilus_common = { path = "../common" }
postcard = { workspace = true }
[dev-dependencies] [dev-dependencies]
embedded-hal-mock = { version = "0.11.1" } embedded-hal-mock = { version = "0.11.1" }

146
flight/src/comms/mod.rs Normal file
View File

@@ -0,0 +1,146 @@
use crate::scheduler::{CyclicTask, TaskHandle};
use anyhow::{Result, ensure};
use log::{error, trace, warn};
use nautilus_common::command::{Command, CommandHeader};
use nautilus_common::telemetry::{Telemetry, TelemetryMessage};
use nautilus_common::udp::{UdpRecvPostcardError, UdpSocketExt};
use std::any::type_name;
use std::collections::HashMap;
use std::fmt::{Debug, Formatter};
use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket};
use std::sync::mpsc::Receiver;
use std::time::Instant;
pub type TelemetrySender = TaskHandle<Telemetry, ()>;
impl TelemetrySender {
pub fn send(&self, telemetry_message: TelemetryMessage) {
trace!("TelemetrySender::send(self: {self:?}, telemetry_message: {telemetry_message:?}");
// Ignore failure
let _ = self.sender.send(Telemetry {
timestamp: chrono::Utc::now(),
message: telemetry_message,
});
}
}
type CommandCallback<'a> = Box<dyn Fn(&[u8]) -> Result<()> + Send + 'a>;
pub struct CommsTask<'a, A>
where
A: ToSocketAddrs + Debug,
{
udp: UdpSocket,
ground_address: A,
command_callbacks: HashMap<String, CommandCallback<'a>>,
}
impl<A> Debug for CommsTask<'_, A>
where
A: ToSocketAddrs + Debug,
{
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(
f,
"CommsTask {{ udp: {:?}, ground_address: {:?} }}",
self.udp, self.ground_address
)
}
}
impl<'a, A> CommsTask<'a, A>
where
A: ToSocketAddrs + Debug,
{
pub fn new(local_port: u16, ground_address: A) -> Result<Self> {
trace!(
"CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})",
type_name::<A>()
);
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,
command_callbacks: HashMap::new(),
})
}
pub fn add_command_handler<T: Command>(
&mut self,
command: impl Into<String>,
handler: impl Fn(T) + Send + 'a,
) -> Result<()> {
let command = command.into();
ensure!(
!self.command_callbacks.contains_key(&command),
"Already Contains Command {command}"
);
self.command_callbacks.insert(
command.clone(),
Box::new(move |cmd_buf| {
let (cmd, remainder) = postcard::take_from_bytes::<T>(cmd_buf)?;
ensure!(
remainder.is_empty(),
"{command} received {} extra bytes",
remainder.len()
);
handler(cmd);
Ok(())
}),
);
Ok(())
}
}
impl<A> CyclicTask for CommsTask<'_, A>
where
A: ToSocketAddrs + Debug,
{
type Message = Telemetry;
type Data = ();
fn get_data(&self) -> Self::Data {
trace!(
"CommsTask<A={}>::get_data(self: {self:?})",
type_name::<A>()
);
}
fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
trace!(
"CommsTask<A={}>::step(self: {self:?}, receiver: {receiver:?}, step_time: {step_time:?})",
type_name::<A>()
);
let mut buffer = [0u8; 512];
match self.udp.recv_postcard::<CommandHeader>(&mut buffer) {
Ok((cmd, _)) => match self.command_callbacks.get(cmd.name) {
Some(handler) => {
if let Err(e) = handler(cmd.data) {
error!("Command Error: {e}");
}
}
None => {
warn!("Unknown Command: {}", cmd.name);
}
},
Err(UdpRecvPostcardError::NoData) => {}
Err(err) => {
error!("Rx error: {err}");
}
}
// Intentionally ignore Err case
while let Ok(tlm) = receiver.try_recv() {
if let Err(err) = self
.udp
.send_postcard(&tlm, &mut buffer, &self.ground_address)
{
error!("Tx Error: {err}");
}
}
}
}

8
flight/src/data/mod.rs
View File

@@ -1,4 +1,4 @@
#[cfg(not(feature = "raspi"))] // #[cfg(not(feature = "raspi"))]
pub mod reader; // pub mod reader;
#[cfg(not(feature = "raspi"))] // #[cfg(not(feature = "raspi"))]
pub mod writer; // pub mod writer;

97
flight/src/hardware/channelization.rs
View File

@@ -1,2 +1,95 @@
pub const MCP23017_A_LED: u8 = 7; #![allow(dead_code)]
pub const MCP23017_B_LED: u8 = 7;
use crate::hardware::pin::{Pin, PinDevice};
use embedded_hal::digital::PinState;
use log::trace;
use std::any::type_name;
use std::fmt::Debug;
use std::time::Instant;
pub const RCS5: PinoutChannel = PinoutChannel::ExtA(0);
pub const RCS6: PinoutChannel = PinoutChannel::ExtA(1);
pub const RCS7: PinoutChannel = PinoutChannel::ExtA(2);
pub const RCS8: PinoutChannel = PinoutChannel::ExtA(3);
pub const RCS9: PinoutChannel = PinoutChannel::ExtA(4);
pub const MOTOR0_A: PinoutChannel = PinoutChannel::ExtA(5);
pub const MOTOR0_B: PinoutChannel = PinoutChannel::ExtA(6);
pub const LED_A: PinoutChannel = PinoutChannel::ExtA(7);
pub const RCS0: PinoutChannel = PinoutChannel::ExtA(8);
pub const RCS1: PinoutChannel = PinoutChannel::ExtA(9);
pub const DRIVE0_BRAKE: PinoutChannel = PinoutChannel::ExtA(10);
pub const DRIVE0_DIR: PinoutChannel = PinoutChannel::ExtA(11);
pub const DRIVE0_DRIVEOFF: PinoutChannel = PinoutChannel::ExtA(12);
pub const RCS2: PinoutChannel = PinoutChannel::ExtA(13);
pub const RCS3: PinoutChannel = PinoutChannel::ExtA(14);
pub const RCS4: PinoutChannel = PinoutChannel::ExtA(15);
pub const MOTOR1_A: PinoutChannel = PinoutChannel::ExtB(8);
pub const MOTOR1_B: PinoutChannel = PinoutChannel::ExtB(9);
pub const MOTOR2_A: PinoutChannel = PinoutChannel::ExtB(10);
pub const MOTOR2_B: PinoutChannel = PinoutChannel::ExtB(11);
pub const MOTOR3_A: PinoutChannel = PinoutChannel::ExtB(12);
pub const MOTOR3_B: PinoutChannel = PinoutChannel::ExtB(13);
pub const OUT1: PinoutChannel = PinoutChannel::ExtB(14);
pub const OUT2: PinoutChannel = PinoutChannel::ExtB(15);
pub const OUT3: PinoutChannel = PinoutChannel::ExtB(0);
pub const OUT4: PinoutChannel = PinoutChannel::ExtB(1);
pub const OUT5: PinoutChannel = PinoutChannel::ExtB(2);
pub const OUT6: PinoutChannel = PinoutChannel::ExtB(3);
pub const OUT7: PinoutChannel = PinoutChannel::ExtB(4);
pub const OUT8: PinoutChannel = PinoutChannel::ExtB(5);
pub const OUT9: PinoutChannel = PinoutChannel::ExtB(6);
pub const LED_B: PinoutChannel = PinoutChannel::ExtB(7);
#[derive(Copy, Clone, Debug)]
pub enum PinoutChannel {
ExtA(u8),
ExtB(u8),
}
pub struct DevicePin<'a, Device: PinDevice> {
pin: u8,
device: &'a Device,
}
impl<Device: PinDevice> Pin for DevicePin<'_, Device> {
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
trace!(
"ChannelPin<Device={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
type_name::<Device>()
);
self.device.set_pin(self.pin, value, valid_until, priority);
}
}
pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> {
ExtA(DevicePin<'a, A>),
ExtB(DevicePin<'a, B>),
}
impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<'_, A, B> {
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
trace!(
"ChannelPin<A={}, B={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
type_name::<A>(),
type_name::<B>()
);
match self {
ChannelPin::ExtA(pin) => pin.set(value, valid_until, priority),
ChannelPin::ExtB(pin) => pin.set(value, valid_until, priority),
}
}
}
impl PinoutChannel {
pub fn get_pin<'a>(
self,
ext_a: &'a (impl PinDevice + Debug),
ext_b: &'a (impl PinDevice + Debug),
) -> impl Pin {
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 }),
PinoutChannel::ExtB(pin) => ChannelPin::ExtB(DevicePin { pin, device: ext_b }),
}
}
}

35
flight/src/hardware/error.rs
View File

@@ -1,35 +0,0 @@
use std::error::Error;
use std::fmt::{Debug, Display, Formatter};
#[derive(Copy, Clone, Debug)]
pub struct I2cError<ERR: Debug + embedded_hal::i2c::Error>(pub ERR);
impl<ERR: Debug + embedded_hal::i2c::Error> Display for I2cError<ERR> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
writeln!(f, "I2cError({:?})", self.0)
}
}
impl<ERR: Debug + embedded_hal::i2c::Error> Error for I2cError<ERR> {}
#[derive(Copy, Clone, Debug)]
pub struct SpiError<ERR: Debug + embedded_hal::spi::Error>(pub ERR);
impl<ERR: Debug + embedded_hal::spi::Error> Display for SpiError<ERR> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
writeln!(f, "SpiError({:?})", self.0)
}
}
impl<ERR: Debug + embedded_hal::spi::Error> Error for SpiError<ERR> {}
// #[derive(Copy, Clone, Debug)]
// pub struct NotAvailableError;
//
// impl Display for NotAvailableError {
// fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
// writeln!(f, "NotAvailableError")
// }
// }
//
// impl Error for NotAvailableError { }

3
flight/src/hardware/error/mod.rs Normal file
View File

@@ -0,0 +1,3 @@
mod wrapping;
pub use wrapping::WrappingError;

26
flight/src/hardware/error/wrapping.rs Normal file
View File

@@ -0,0 +1,26 @@
use embedded_hal::i2c::ErrorKind;
use std::error::Error;
use std::fmt::{Debug, Display, Formatter};
#[derive(Copy, Clone, Debug)]
pub struct WrappingError<ERR: Debug>(pub ERR);
impl<ERR: Debug> Display for WrappingError<ERR> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
writeln!(f, "WrappingError({:?})", self.0)
}
}
impl<ERR: Debug> Error for WrappingError<ERR> {}
impl<ERR: Debug> embedded_hal::i2c::Error for WrappingError<ERR> {
fn kind(&self) -> ErrorKind {
ErrorKind::Other
}
}
impl<ERR: Debug> embedded_hal::pwm::Error for WrappingError<ERR> {
fn kind(&self) -> embedded_hal::pwm::ErrorKind {
embedded_hal::pwm::ErrorKind::Other
}
}

100
flight/src/hardware/mcp23017/driver.rs
View File

@@ -1,11 +1,11 @@
use crate::hardware::error::I2cError; use crate::hardware::error::WrappingError;
use crate::hardware::mcp23017::pin::{Mcp23017OutputPinDriver, Mcp23017Pins}; use crate::hardware::mcp23017::Mcp23017;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin};
use anyhow::bail; use anyhow::bail;
use embedded_hal::digital::PinState;
use embedded_hal::i2c::I2c; use embedded_hal::i2c::I2c;
use log::{error, trace}; use log::{error, trace};
use std::any::type_name;
use std::fmt::{Debug, Formatter}; use std::fmt::{Debug, Formatter};
use std::sync::atomic::{AtomicU16, Ordering};
use std::sync::Mutex; use std::sync::Mutex;
pub struct Mcp23017Driver<I2C> pub struct Mcp23017Driver<I2C>
@@ -17,9 +17,8 @@ where
{ {
i2c: Mutex<I2C>, i2c: Mutex<I2C>,
address: u8, address: u8,
bank: AtomicU16, bank: u16,
last_flushed: AtomicU16, last_flushed: u16,
in_use: AtomicU16,
} }
impl<I2C> Debug for Mcp23017Driver<I2C> impl<I2C> Debug for Mcp23017Driver<I2C>
@@ -30,7 +29,12 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mcp23017Driver {{ address: {} }}", self.address) write!(
f,
"Mcp23017Driver<I2C={}> {{ address: {} }}",
type_name::<I2C>(),
self.address
)
} }
} }
@@ -42,13 +46,15 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
pub fn new(i2c: I2C, address: u8) -> Self { pub fn new(i2c: I2C, address: u8) -> Self {
trace!("Mcp23017Driver::new(i2c, address: 0x{address:02x})"); trace!(
"Mcp23017Driver<I2C={}>::new(i2c, address: 0x{address:02x})",
type_name::<I2C>()
);
Self { Self {
i2c: i2c.into(), i2c: i2c.into(),
address, address,
bank: AtomicU16::new(0), bank: 0,
last_flushed: AtomicU16::new(0), last_flushed: 0,
in_use: AtomicU16::new(0),
} }
} }
} }
@@ -62,40 +68,13 @@ where
{ {
fn drop(&mut self) { fn drop(&mut self) {
trace!("Mcp23017Driver::drop(self: {self:?})"); trace!("Mcp23017Driver::drop(self: {self:?})");
self.bank = 0; // We want all pins to be set back to 0
if let Err(e) = self.flush() { if let Err(e) = self.flush() {
error!("Mcp23017Driver: Failed to flush on drop. {self:?} Error: {e}") error!("Mcp23017Driver: Failed to flush on drop. {self:?} Error: {e}");
} }
} }
} }
impl<I2C> Mcp23017Pins for Mcp23017Driver<I2C>
where
I2C: I2c + Send + Sync,
I2C::Error: Send,
I2C::Error: Sync,
I2C::Error: 'static,
{
fn set_pin(&self, pin: u8, value: bool) {
trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value})");
let pin_mask = 1u16 << pin;
if value {
// Relaxed because we don't care about the value
self.bank.fetch_or(pin_mask, Ordering::Relaxed);
} else {
// Relaxed because we don't care about the value
self.bank.fetch_and(!pin_mask, Ordering::Relaxed);
}
}
fn release_pin(&self, pin: u8) {
trace!("Mcp23017Driver::release_pin(self: {self:?}, pin: {pin})");
let pin_mask = 1u16 << pin;
// Sequentially Consistent because we want all accesses here
// to have a single modification order
self.in_use.fetch_and(!pin_mask, Ordering::SeqCst);
}
}
impl<I2C> Mcp23017 for Mcp23017Driver<I2C> impl<I2C> Mcp23017 for Mcp23017Driver<I2C>
where where
I2C: I2c + Send + Sync, I2C: I2c + Send + Sync,
@@ -109,49 +88,38 @@ where
let data: [u8; _] = [0x00, 0x00, 0x00]; let data: [u8; _] = [0x00, 0x00, 0x00];
match self.i2c.get_mut() { match self.i2c.get_mut() {
Ok(lock) => lock.write(self.address, &data).map_err(I2cError)?, Ok(lock) => lock.write(self.address, &data).map_err(WrappingError)?,
Err(_) => bail!("Lock was poisoned"), Err(_) => bail!("Lock was poisoned"),
} }
Ok(()) Ok(())
} }
fn new_output_pin(&self, pin: u8) -> anyhow::Result<impl Mcp23017OutputPin> { fn set_pin(&mut self, pin: u8, value: PinState) -> anyhow::Result<()> {
trace!("Mcp23017Driver::new_output_pin(self: {self:?}, pin: {pin})"); trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
if !(0u8..16u8).contains(&pin) { if !(0u8..16u8).contains(&pin) {
bail!("Invalid Pin ID") bail!("Invalid Pin ID")
} }
let pin_mask = 1u16 << pin; let pin_mask = 1u16 << pin;
// Sequentially Consistent because we want all accesses here
// to have a single modification order
let previous_value = self.in_use.fetch_or(pin_mask, Ordering::SeqCst);
if (previous_value & pin_mask) != 0 {
// If the pin was previously enabled
bail!("Output Pin Already Exists");
}
Ok(Mcp23017OutputPinDriver { self.bank = match value {
mcp23017: self, PinState::Low => self.bank & !pin_mask,
pin, PinState::High => self.bank | pin_mask,
on_drop: None, };
})
Ok(())
} }
fn flush(&self) -> anyhow::Result<()> { fn flush(&mut self) -> anyhow::Result<()> {
trace!("Mcp23017Driver::flush(self: {self:?})"); trace!("Mcp23017Driver::flush(self: {self:?})");
// Relaxed because we don't care about the value if self.bank != self.last_flushed {
let bank = self.bank.load(Ordering::Relaxed); let bytes = self.bank.to_le_bytes();
// Acquire-Release because we want all previous writes to be visible
let last_flushed = self.last_flushed.swap(bank, Ordering::AcqRel);
let dirty = bank != last_flushed;
if dirty {
let bytes = bank.to_le_bytes();
let data: [u8; _] = [0x12, bytes[0], bytes[1]]; let data: [u8; _] = [0x12, bytes[0], bytes[1]];
// This blocks while writing // This blocks while writing
if let Ok(mut lock) = self.i2c.lock() { if let Ok(mut lock) = self.i2c.lock() {
lock.write(self.address, &data).map_err(I2cError)?; lock.write(self.address, &data).map_err(WrappingError)?;
self.i2c.clear_poison(); self.i2c.clear_poison();
self.last_flushed = self.bank;
} }
} }
Ok(()) Ok(())

12
flight/src/hardware/mcp23017/mod.rs
View File

@@ -1,4 +1,3 @@
mod pin;
mod driver; mod driver;
mod task; mod task;
@@ -8,16 +7,11 @@ use embedded_hal::digital::PinState;
pub trait Mcp23017 { pub trait Mcp23017 {
fn init(&mut self) -> Result<()>; fn init(&mut self) -> Result<()>;
fn new_output_pin(&self, pin: u8) -> Result<impl Mcp23017OutputPin>; fn set_pin(&mut self, pin: u8, value: PinState) -> Result<()>;
fn flush(&self) -> Result<()>; fn flush(&mut self) -> Result<()>;
}
pub trait Mcp23017OutputPin {
fn set_state(&mut self, pin_state: PinState);
fn set_state_on_drop(&mut self, pin_state: PinState);
} }
pub use driver::Mcp23017Driver; pub use driver::Mcp23017Driver;
pub use task::Mcp23017State;
pub use task::Mcp23017Task; pub use task::Mcp23017Task;

44
flight/src/hardware/mcp23017/pin.rs
View File

@@ -1,44 +0,0 @@
use crate::hardware::mcp23017::Mcp23017OutputPin;
use embedded_hal::digital::PinState;
use log::trace;
use std::fmt::{Debug, Formatter};
pub(super) trait Mcp23017Pins {
fn set_pin(&self, pin: u8, value: bool);
fn release_pin(&self, pin: u8);
}
pub struct Mcp23017OutputPinDriver<'a, Device: Mcp23017Pins> {
pub(super) mcp23017: &'a Device,
pub(super) pin: u8,
pub(super) on_drop: Option<PinState>,
}
impl<'a, Device: Mcp23017Pins> Debug for Mcp23017OutputPinDriver<'a, Device> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mcp23017OutputPin {{ pin: {} }}", self.pin)
}
}
impl<'a, Device: Mcp23017Pins> Mcp23017OutputPin for Mcp23017OutputPinDriver<'a, Device> {
fn set_state(&mut self, pin_state: PinState) {
trace!("Mcp23017OutputPin::set_state(self: {self:?}, pin_state: {pin_state:?})");
self.mcp23017.set_pin(self.pin, pin_state.into())
}
fn set_state_on_drop(&mut self, pin_state: PinState) {
trace!("Mcp23017OutputPin::set_state_on_drop(self: {self:?}, pin_state: {pin_state:?})");
self.on_drop = Some(pin_state);
}
}
impl<'a, Device: Mcp23017Pins> Drop for Mcp23017OutputPinDriver<'a, Device> {
fn drop(&mut self) {
trace!("Mcp23017OutputPin::drop(self: {self:?})");
if let Some(pin_state) = self.on_drop {
self.mcp23017.set_pin(self.pin, pin_state.into());
}
self.mcp23017.release_pin(self.pin);
}
}

251
flight/src/hardware/mcp23017/task.rs
View File

@@ -1,36 +1,235 @@
use crate::hardware::mcp23017::Mcp23017; use crate::hardware::mcp23017::Mcp23017;
use anyhow::Result; use crate::hardware::pin::PinDevice;
use std::sync::atomic::{AtomicBool, Ordering}; use crate::scheduler::{CyclicTask, TaskHandle};
use std::thread; use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
use std::thread::{sleep, Scope, ScopedJoinHandle}; use embedded_hal::digital::PinState;
use std::time::Duration; use log::trace;
use std::fmt::{Debug, Formatter};
use std::sync::mpsc::Receiver;
use std::time::Instant;
pub struct Mcp23017Task<'a, M: Mcp23017 + Sync> { #[derive(Clone, Debug)]
mcp23017: &'a M, pub enum Mcp23017Message {
name: String, SetPin {
pin: u8,
value: PinState,
valid_until: Instant,
priority: u8,
},
} }
impl<'a, M: Mcp23017 + Sync> Mcp23017Task<'a, M> { impl<D: Debug> PinDevice for TaskHandle<Mcp23017Message, D> {
pub fn new( fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) {
mcp23017: &'a M, trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
name: String, // This can only fail if the other end is disconnected - which we intentionally want to
) -> Self { // ignore
let _ = self.sender.send(Mcp23017Message::SetPin {
pin,
value,
valid_until,
priority,
});
}
}
#[derive(Debug, Default)]
pub struct Mcp23017State {
pub pins: [bool; 16],
}
#[derive(Debug, Clone)]
pub struct Mcp23017Data {
id: SectionIdentifier,
}
impl Mcp23017Data {
pub fn get_id(&self) -> SectionIdentifier {
self.id.clone()
}
}
pub struct Mcp23017Task<'a, M: Mcp23017> {
mcp23017: M,
pins: AllPins,
state: SectionWriter<'a, Mcp23017State>,
}
impl<M: Mcp23017 + Debug> Debug for Mcp23017Task<'_, M> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mcp23017Task {{ mcp23017: {:?} }}", self.mcp23017)
}
}
struct AllPins {
pins: [PinData; 16],
}
impl AllPins {
fn new() -> Self {
trace!("AllPins::new()");
Self { Self {
mcp23017, pins: [PinData::new(); _],
name, }
}
}
#[derive(Copy, Clone, Debug)]
struct PinData {
state: PinState,
valid_until: Option<Instant>,
priority: u8,
next_state: PinState,
next_validity: Option<Instant>,
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,
} }
} }
pub fn start(self, scope: &'a Scope<'a, '_>, running: &'a AtomicBool, frequency: u64) -> Result<ScopedJoinHandle<'a, ()>> { fn evaluate(&mut self, now: Instant) {
let period = Duration::from_nanos(1_000_000_000 / frequency); 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;
}
Ok(thread::Builder::new() if self.valid_until.is_some() {
.name(self.name) self.changed = true;
.spawn_scoped(scope, move || { }
while running.load(Ordering::Relaxed) { self.state = self.next_state;
let _ = self.mcp23017.flush(); self.valid_until = self.next_validity;
sleep(period); 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
}
}
}
}
impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self {
trace!("Mcp23017Task::new(mcp23017: {mcp23017:?})");
Self {
mcp23017,
pins: AllPins::new(),
state: state_vector.create_section(Mcp23017State::default()),
}
}
}
impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
type Message = Mcp23017Message;
type Data = Mcp23017Data;
fn get_data(&self) -> Self::Data {
trace!("Mcp23017Task::get_data(self: {self:?})");
Self::Data {
id: self.state.get_identifier(),
}
}
fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})");
let mut changed = false;
for pin in 0u8..16u8 {
self.pins.pins[pin as usize].evaluate(step_time);
}
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);
}
}
}
}
for pin in 0u8..16u8 {
let state = self.pins.pins[pin as usize].value;
if self.pins.pins[pin as usize].changed {
self.pins.pins[pin as usize].changed = false;
// This shouldn't be able to fail
// TODO: handle error case
let _ = self.mcp23017.set_pin(pin, state);
changed = true;
}
}
if changed {
let _ = self.mcp23017.flush();
self.state.update(|s| {
s.pins = self.pins.pins.map(|pin| pin.value == PinState::High);
});
}
}
}

28
flight/src/hardware/mcp3208/mod.rs
View File

@@ -1,7 +1,8 @@
use crate::hardware::error::SpiError; use crate::hardware::error::WrappingError;
use anyhow::{ensure, Result}; use anyhow::{Result, ensure};
use embedded_hal::spi::SpiDevice; use embedded_hal::spi::SpiDevice;
use log::trace; use log::trace;
use std::any::type_name;
use std::fmt::{Debug, Formatter}; use std::fmt::{Debug, Formatter};
pub struct Mcp3208<SPI> { pub struct Mcp3208<SPI> {
@@ -11,7 +12,12 @@ pub struct Mcp3208<SPI> {
impl<SPI> Debug for Mcp3208<SPI> { impl<SPI> Debug for Mcp3208<SPI> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mcp3208 {{ vref: {} }}", self.vref) write!(
f,
"Mcp3208<SPI={}> {{ vref: {} }}",
type_name::<SPI>(),
self.vref
)
} }
} }
@@ -23,11 +29,11 @@ where
SPI::Error: 'static, SPI::Error: 'static,
{ {
pub fn new(spi: SPI, vref: f64) -> Self { pub fn new(spi: SPI, vref: f64) -> Self {
trace!("Mcp3208::new(spi, vref: {vref})"); trace!(
Self { "Mcp3208<SPI={}>::new(spi, vref: {vref})",
spi, type_name::<SPI>()
vref, );
} Self { spi, vref }
} }
pub fn read_single(&mut self, channel: u8) -> Result<f64> { pub fn read_single(&mut self, channel: u8) -> Result<f64> {
@@ -40,10 +46,12 @@ where
write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF; write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF;
let mut read_bits = [0u8; 3]; let mut read_bits = [0u8; 3];
self.spi.transfer(&mut read_bits, &write_bits).map_err(SpiError)?; self.spi
.transfer(&mut read_bits, &write_bits)
.map_err(WrappingError)?;
let value: u16 = u16::from_be_bytes([read_bits[1], read_bits[2]]) & 0x0FFF; let value: u16 = u16::from_be_bytes([read_bits[1], read_bits[2]]) & 0x0FFF;
Ok(((value as f64) / (0xFFF as f64)) * self.vref) Ok(((value as f64) / (0xFFF as f64)) * self.vref)
} }
} }

3
flight/src/hardware/mct8316a/closed_loop.rs
View File

@@ -249,7 +249,6 @@ pub enum DutyCycleThreshold {
Percent2_5 = 0x7, Percent2_5 = 0x7,
} }
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct ClosedLoop3 { pub struct ClosedLoop3 {
pub degauss_samples: DegaussSamples, pub degauss_samples: DegaussSamples,
@@ -481,5 +480,3 @@ pub enum FastBrakeDelta {
Percent4 = 0x6, Percent4 = 0x6,
Percent5 = 0x7, Percent5 = 0x7,
} }

1
flight/src/hardware/mct8316a/constant_power.rs
View File

@@ -35,4 +35,3 @@ pub enum PowerMode {
ClosedLoop = 0x1, ClosedLoop = 0x1,
PowerLimit = 0x2, PowerLimit = 0x2,
} }

150
flight/src/hardware/mct8316a/driver.rs
View File

@@ -1,20 +1,60 @@
use crate::hardware::error::I2cError; use crate::hardware::error::WrappingError;
use crate::hardware::mct8316a::closed_loop::{BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode, ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples, DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold, IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold, LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode, PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode, UpperPercentLimit}; use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::mct8316a::closed_loop::{
BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode,
ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples,
DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold,
IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold,
LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode,
PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode,
UpperPercentLimit,
};
use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode}; use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode};
use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed}; use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed};
use crate::hardware::mct8316a::device_config::{ClockSource, DeviceConfig, DeviceMode, ExternalClockFrequency, PwmRangeSelect}; use crate::hardware::mct8316a::device_config::{
ClockSource, DeviceConfig, DeviceMode, ExternalClockFrequency, PwmRangeSelect,
};
use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom; use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom;
use crate::hardware::mct8316a::fault_config::{AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit, FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime, LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage, MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold}; use crate::hardware::mct8316a::fault_config::{
use crate::hardware::mct8316a::gate_driver_config::{BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier, DemagComparatorThreshold, GateDriverConfig1, GateDriverConfig2, OvercurrentFaultMode, OvercurrentProtectionDeglitchTime, OvercurrentProtectionLevel, OvercurrentProtectionRetryTime, OvervoltageProtectionLevel, SlewRate, TargetDelay}; AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit,
use crate::hardware::mct8316a::isd_config::{BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold, StartupBreakTime, Threshold}; FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime,
use crate::hardware::mct8316a::motor_startup::{AlignRampRate, AlignTime, DutyCycle, FirstCycleFrequencySelect, FullCurrentThreshold, IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat, MinimumDutyCycle, MotorStartup1, MotorStartup2, MotorStartupMethod, OpenClosedHandoffThreshold, OpenLoopAcceleration1, OpenLoopAcceleration2, OpenLoopCurrentLimitMode, SlowFirstCycleFrequency}; LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage,
use crate::hardware::mct8316a::phase_profile::{ProfileSetting, ThreePhase150DegreeProfile, ThreePhaseLeadAngle, TwoPhase150DegreeProfile}; MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold,
use crate::hardware::mct8316a::pin_config::{BrakeInputConfig, DirectionInputConfig, ExternalWatchdogFaultMode, ExternalWatchdogFrequency, ExternalWatchdogSource, Pin36Config, Pin37_38Config, PinConfig1, PinConfig2, SleepTime, SpeedInputConfig}; };
use crate::hardware::mct8316a::trap_config::{AVSLimitHysteresis, AVSNegativeCurrentLimit, FastStartupDivFactor, IsdBemfThreshold, IsdCycleThreshold, OpenLoopHandoffCycles, OpenLoopZcDetectionThreshold, TrapConfig1, TrapConfig2}; use crate::hardware::mct8316a::gate_driver_config::{
use crate::hardware::mct8316a::Mct8316a; BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier,
use anyhow::{bail, ensure, Result}; DemagComparatorThreshold, GateDriverConfig1, GateDriverConfig2, OvercurrentFaultMode,
OvercurrentProtectionDeglitchTime, OvercurrentProtectionLevel, OvercurrentProtectionRetryTime,
OvervoltageProtectionLevel, SlewRate, TargetDelay,
};
use crate::hardware::mct8316a::isd_config::{
BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold,
StartupBreakTime, Threshold,
};
use crate::hardware::mct8316a::motor_startup::{
AlignRampRate, AlignTime, DutyCycle, FirstCycleFrequencySelect, FullCurrentThreshold,
IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat,
MinimumDutyCycle, MotorStartup1, MotorStartup2, MotorStartupMethod, OpenClosedHandoffThreshold,
OpenLoopAcceleration1, OpenLoopAcceleration2, OpenLoopCurrentLimitMode,
SlowFirstCycleFrequency,
};
use crate::hardware::mct8316a::phase_profile::{
ProfileSetting, ThreePhase150DegreeProfile, ThreePhaseLeadAngle, TwoPhase150DegreeProfile,
};
use crate::hardware::mct8316a::pin_config::{
BrakeInputConfig, DirectionInputConfig, ExternalWatchdogFaultMode, ExternalWatchdogFrequency,
ExternalWatchdogSource, Pin36Config, Pin37_38Config, PinConfig1, PinConfig2, SleepTime,
SpeedInputConfig,
};
use crate::hardware::mct8316a::trap_config::{
AVSLimitHysteresis, AVSNegativeCurrentLimit, FastStartupDivFactor, IsdBemfThreshold,
IsdCycleThreshold, OpenLoopHandoffCycles, OpenLoopZcDetectionThreshold, TrapConfig1,
TrapConfig2,
};
use anyhow::{Result, bail, ensure};
use embedded_hal::i2c::{I2c, Operation}; use embedded_hal::i2c::{I2c, Operation};
use log::trace; use log::trace;
use std::any::type_name;
use std::fmt::{Debug, Display, Formatter}; use std::fmt::{Debug, Display, Formatter};
use std::sync::Mutex; use std::sync::Mutex;
@@ -38,6 +78,7 @@ impl Display for Mct8316AVData {
} }
} }
#[derive(Debug, Copy, Clone)]
enum OperationRW { enum OperationRW {
Read, Read,
Write, Write,
@@ -49,16 +90,16 @@ fn control_word(
data: Mct8316AVData, data: Mct8316AVData,
address: u32, address: u32,
) -> [u8; 3] { ) -> [u8; 3] {
trace!(
"control_word(operation_rw: {operation_rw:?}, crc: {crc}, data: {data}, address: {address:06x})"
);
let mut control_word = [0u8; _]; let mut control_word = [0u8; _];
control_word[0] |= match operation_rw { control_word[0] |= match operation_rw {
OperationRW::Read => 0x80, OperationRW::Read => 0x80,
OperationRW::Write => 0x00, OperationRW::Write => 0x00,
}; };
control_word[0] |= match crc { control_word[0] |= if crc { 0x40 } else { 0x00 };
true => 0x40,
false => 0x00,
};
control_word[0] |= match data { control_word[0] |= match data {
Mct8316AVData::Two(_) => 0x00, Mct8316AVData::Two(_) => 0x00,
Mct8316AVData::Four(_) => 0x10, Mct8316AVData::Four(_) => 0x10,
@@ -66,7 +107,7 @@ fn control_word(
}; };
control_word[0] |= ((address >> 16) & 0x0F) as u8; control_word[0] |= ((address >> 16) & 0x0F) as u8;
control_word[1] |= ((address >> 8) & 0xFF) as u8; control_word[1] |= ((address >> 8) & 0xFF) as u8;
control_word[2] |= ((address >> 0) & 0xFF) as u8; control_word[2] |= ((address) & 0xFF) as u8;
control_word control_word
} }
@@ -90,7 +131,12 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mct8316AVDriver {{ address: {} }}", self.address) write!(
f,
"Mct8316AVDriver<I2C={}> {{ address: {} }}",
type_name::<I2C>(),
self.address
)
} }
} }
@@ -102,13 +148,17 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
pub fn new(i2c: I2C, address: u8) -> Self { pub fn new(i2c: I2C, address: u8) -> Self {
trace!("Mct8316AVDriver::new(i2c, address: 0x{address:02x})"); trace!(
"Mct8316AVDriver<I2C={}>::new(i2c, address: 0x{address:02x})",
type_name::<I2C>()
);
Self { Self {
i2c: i2c.into(), i2c: i2c.into(),
address, address,
} }
} }
#[allow(clippy::identity_op)]
pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> { pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})"); trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");
@@ -118,12 +168,7 @@ where
// 1 for crc // 1 for crc
let mut write_data = [0u8; 1 + 3 + 8 + 1]; let mut write_data = [0u8; 1 + 3 + 8 + 1];
write_data[0] = (self.address << 1) | 0b0; write_data[0] = (self.address << 1) | 0b0;
write_data[1..4].copy_from_slice(&control_word( write_data[1..4].copy_from_slice(&control_word(OperationRW::Write, true, data, address));
OperationRW::Write,
true,
data,
address,
));
let data_length = match data { let data_length = match data {
Mct8316AVData::Two(val) => { Mct8316AVData::Two(val) => {
write_data[4..6].copy_from_slice(&val.to_be_bytes()); write_data[4..6].copy_from_slice(&val.to_be_bytes());
@@ -142,13 +187,16 @@ where
write_data[4 + data_length] = crc_result; write_data[4 + data_length] = crc_result;
match self.i2c.lock() { match self.i2c.lock() {
Ok(mut lock) => lock.write(self.address, &write_data).map_err(I2cError)?, Ok(mut lock) => lock
.write(self.address, &write_data[1..=(4 + data_length)])
.map_err(WrappingError)?,
Err(_) => bail!("Lock was poisoned"), Err(_) => bail!("Lock was poisoned"),
} }
Ok(()) Ok(())
} }
#[allow(clippy::identity_op)]
pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> { pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})"); trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
// 1 target id // 1 target id
@@ -157,13 +205,9 @@ where
// 8 data bytes // 8 data bytes
// 1 crc // 1 crc
let mut read_data = [0u8; 1 + 3 + 1 + 8 + 1]; let mut read_data = [0u8; 1 + 3 + 1 + 8 + 1];
// Least significant bit should be 0
read_data[0] = (self.address << 1) | 0b0; read_data[0] = (self.address << 1) | 0b0;
read_data[1..4].copy_from_slice(&control_word( read_data[1..4].copy_from_slice(&control_word(OperationRW::Read, true, *data, address));
OperationRW::Read,
true,
*data,
address,
));
read_data[4] = (self.address << 1) | 0b1; read_data[4] = (self.address << 1) | 0b1;
let data_length = match data { let data_length = match data {
Mct8316AVData::Two(_) => 2, Mct8316AVData::Two(_) => 2,
@@ -175,32 +219,42 @@ where
let mut i2c_ops = [ let mut i2c_ops = [
Operation::Write(&left[1..4]), Operation::Write(&left[1..4]),
Operation::Read(&mut right[..(data_length + 1)]) Operation::Read(&mut right[..=data_length]),
]; ];
match self.i2c.lock() { match self.i2c.lock() {
Ok(mut lock) => lock.transaction(self.address, &mut i2c_ops).map_err(I2cError)?, Ok(mut lock) => lock
.transaction(self.address, &mut i2c_ops)
.map_err(WrappingError)?,
Err(_) => bail!("Lock was poisoned"), Err(_) => bail!("Lock was poisoned"),
} }
drop(i2c_ops);
let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]); let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]);
ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch"); ensure!(
expected_crc == read_data[5 + data_length],
"CRC Mismatch. {expected_crc} expected. {}",
read_data[5 + data_length]
);
match data { match data {
Mct8316AVData::Two(val) => { Mct8316AVData::Two(val) => {
*val = u16::from_be_bytes([read_data[5], read_data[6]]); *val = u16::from_be_bytes([read_data[5], read_data[6]]);
} }
Mct8316AVData::Four(val) => { Mct8316AVData::Four(val) => {
*val = u32::from_be_bytes([read_data[5], read_data[6], *val = u32::from_be_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
read_data[7], read_data[8]]);
} }
Mct8316AVData::Eight(val) => { Mct8316AVData::Eight(val) => {
*val = u64::from_be_bytes([read_data[5], read_data[6], *val = u64::from_be_bytes([
read_data[7], read_data[8], read_data[5],
read_data[9], read_data[10], read_data[6],
read_data[11], read_data[12]]); read_data[7],
read_data[8],
read_data[9],
read_data[10],
read_data[11],
read_data[12],
]);
} }
} }
@@ -215,6 +269,7 @@ where
I2C::Error: Sync, I2C::Error: Sync,
I2C::Error: 'static, I2C::Error: 'static,
{ {
#[allow(clippy::too_many_lines)]
fn init(&mut self) -> Result<()> { fn init(&mut self) -> Result<()> {
trace!("Mct8316AVDriver::init(self: {self:?})"); trace!("Mct8316AVDriver::init(self: {self:?})");
@@ -309,7 +364,7 @@ where
})? })?
.set_constant_speed(ConstantSpeed { .set_constant_speed(ConstantSpeed {
speed_power_kp: 0.001f64, speed_power_kp: 0.001f64,
speed_power_ki: 0.000005f64, speed_power_ki: 0.000_005_f64,
speed_power_upper_limit: UpperPercentLimit::Percent100, speed_power_upper_limit: UpperPercentLimit::Percent100,
speed_power_lower_limit: LowerPercentLimit::Percent10, speed_power_lower_limit: LowerPercentLimit::Percent10,
mode: ClosedLoopMode::Disabled, mode: ClosedLoopMode::Disabled,
@@ -362,10 +417,12 @@ where
})? })?
.set_fault_config1(FaultConfig1 { .set_fault_config1(FaultConfig1 {
no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds100, no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds100,
cycle_by_cycle_current_limit: CycleByCycleCurrentLimit::RecoverNextPwmFaultInactiveRecirculation, cycle_by_cycle_current_limit:
CycleByCycleCurrentLimit::RecoverNextPwmFaultInactiveRecirculation,
lock_detection_current_limit: LockDetectionCurrentLimit::Volts1_4, lock_detection_current_limit: LockDetectionCurrentLimit::Volts1_4,
lock_detection_current_limit_mode: LockMode::ReportOnly, lock_detection_current_limit_mode: LockMode::ReportOnly,
lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime::Milliseconds75, lock_detection_current_limit_deglitch_time:
LockDetectionCurrentLimitDeglitchTime::Milliseconds75,
cycle_by_cycle_pwm_limit: 0, cycle_by_cycle_pwm_limit: 0,
motor_lock_mode: LockMode::RecoverRetryTristated, motor_lock_mode: LockMode::RecoverRetryTristated,
lock_retry_time: LockRetryTime::Milliseconds5000, lock_retry_time: LockRetryTime::Milliseconds5000,
@@ -418,7 +475,8 @@ where
overvoltage_protection_level: OvervoltageProtectionLevel::Volts32, overvoltage_protection_level: OvervoltageProtectionLevel::Volts32,
overvoltage_protection_enable: true, overvoltage_protection_enable: true,
overtemperature_warning_enable: false, overtemperature_warning_enable: false,
overcurrent_protection_deglitch_time: OvercurrentProtectionDeglitchTime::Microsecond0_2, overcurrent_protection_deglitch_time:
OvercurrentProtectionDeglitchTime::Microsecond0_2,
overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5, overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5,
overcurrent_protection_level: OvercurrentProtectionLevel::Amps16, overcurrent_protection_level: OvercurrentProtectionLevel::Amps16,
overcurrent_fault_mode: OvercurrentFaultMode::Latch, overcurrent_fault_mode: OvercurrentFaultMode::Latch,

601
flight/src/hardware/mct8316a/eeprom.rs
View File

@@ -1,3 +1,4 @@
use crate::hardware::mct8316a::Mct8316AVDriver;
use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4}; use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
use crate::hardware::mct8316a::constant_power::ConstantPower; use crate::hardware::mct8316a::constant_power::ConstantPower;
use crate::hardware::mct8316a::constant_speed::ConstantSpeed; use crate::hardware::mct8316a::constant_speed::ConstantSpeed;
@@ -7,13 +8,16 @@ use crate::hardware::mct8316a::fault_config::{FaultConfig1, FaultConfig2};
use crate::hardware::mct8316a::gate_driver_config::{GateDriverConfig1, GateDriverConfig2}; use crate::hardware::mct8316a::gate_driver_config::{GateDriverConfig1, GateDriverConfig2};
use crate::hardware::mct8316a::isd_config::IsdConfig; use crate::hardware::mct8316a::isd_config::IsdConfig;
use crate::hardware::mct8316a::motor_startup::{MotorStartup1, MotorStartup2}; use crate::hardware::mct8316a::motor_startup::{MotorStartup1, MotorStartup2};
use crate::hardware::mct8316a::phase_profile::{ThreePhase150DegreeProfile, TwoPhase150DegreeProfile}; use crate::hardware::mct8316a::phase_profile::{
ThreePhase150DegreeProfile, TwoPhase150DegreeProfile,
};
use crate::hardware::mct8316a::pin_config::{PinConfig1, PinConfig2}; use crate::hardware::mct8316a::pin_config::{PinConfig1, PinConfig2};
use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2}; use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2};
use crate::hardware::mct8316a::Mct8316AVDriver;
use anyhow::Result; use anyhow::Result;
use embedded_hal::i2c::I2c; use embedded_hal::i2c::I2c;
use log::trace; use log::trace;
use std::any::type_name;
use std::fmt::{Debug, Formatter};
use std::thread::sleep; use std::thread::sleep;
use std::time::Duration; use std::time::Duration;
@@ -28,6 +32,26 @@ where
modified: bool, modified: bool,
} }
impl<I2C> Debug for Mct8316AVEeprom<'_, I2C>
where
I2C: I2c + Send + Sync,
I2C::Error: Send,
I2C::Error: Sync,
I2C::Error: 'static,
{
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(
f,
"Mct8316AVEeprom<I2C={}> {{ driver: {:?}, modified: {} }}",
type_name::<I2C>(),
self.driver,
self.modified
)
}
}
// We intentionally pass by value in here
#[allow(clippy::needless_pass_by_value)]
impl<'a, I2C> Mct8316AVEeprom<'a, I2C> impl<'a, I2C> Mct8316AVEeprom<'a, I2C>
where where
I2C: I2c + Send + Sync, I2C: I2c + Send + Sync,
@@ -36,9 +60,12 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
pub fn load(driver: &'a mut Mct8316AVDriver<I2C>) -> Result<Self> { pub fn load(driver: &'a mut Mct8316AVDriver<I2C>) -> Result<Self> {
trace!("Mct8316AVEeprom::load()"); trace!(
"Mct8316AVEeprom<I2C={}>::load(driver: {driver:?})",
type_name::<I2C>()
);
driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; driver.write(0x00_00E6, Mct8316AVData::Four(0x4000_0000))?;
// Wait 100ms for the EEPROM operation to complete // Wait 100ms for the EEPROM operation to complete
sleep(Duration::from_millis(100)); sleep(Duration::from_millis(100));
@@ -49,349 +76,470 @@ where
} }
pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> { pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> {
trace!("Mct8316AVEeprom::set_isd_config(isd_config: {isd_config:?})"); trace!("Mct8316AVEeprom::set_isd_config(self: {self:?}, isd_config: {isd_config:?})");
let expected_value = let expected_value = if isd_config.enable_isd {
if isd_config.enable_isd { 0x40000000 } else { 0 } 0x4000_0000
| if isd_config.enable_brake { 0x20000000 } else { 0 } } else {
| if isd_config.enable_high_impedance { 0x10000000 } else { 0 } 0
| if isd_config.enable_reverse_drive { 0x08000000 } else { 0 } } | if isd_config.enable_brake {
| if isd_config.enable_resynchronization { 0x04000000 } else { 0 } 0x2000_0000
| if isd_config.enable_stationary_brake { 0x02000000 } else { 0 } } else {
| (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22) 0
| ((isd_config.brake_mode as u32) & 0x1) << 21 } | if isd_config.enable_high_impedance {
| ((isd_config.brake_config as u32) & 0x1) << 20 0x1000_0000
| ((isd_config.brake_current_threshold as u32) & 0x7) << 17 } else {
| ((isd_config.brake_time as u32) & 0xF) << 13 0
| ((isd_config.high_impedence_time as u32) & 0xF) << 9 } | if isd_config.enable_reverse_drive {
| ((isd_config.startup_break_time as u32) & 0x7) << 6 0x0800_0000
| ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3; } else {
0
} | if isd_config.enable_resynchronization {
0x0400_0000
} else {
0
} | if isd_config.enable_stationary_brake {
0x0200_0000
} else {
0
} | (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22)
| ((isd_config.brake_mode as u32) & 0x1) << 21
| ((isd_config.brake_config as u32) & 0x1) << 20
| ((isd_config.brake_current_threshold as u32) & 0x7) << 17
| ((isd_config.brake_time as u32) & 0xF) << 13
| ((isd_config.high_impedence_time as u32) & 0xF) << 9
| ((isd_config.startup_break_time as u32) & 0x7) << 6
| ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3;
self.assert_register(0x80, Mct8316AVData::Four(expected_value)) self.assert_register(0x80, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result<Self> { pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_motor_startup1(motor_startup1: {motor_startup1:?})"); trace!(
"Mct8316AVEeprom::set_motor_startup1(self: {self:?}, motor_startup1: {motor_startup1:?})"
);
let expected_value = let expected_value = ((motor_startup1.motor_startup_method as u32) & 0x3) << 29
((motor_startup1.motor_startup_method as u32) & 0x3) << 29 | ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25 | ((motor_startup1.align_time as u32) & 0xF) << 21
| ((motor_startup1.align_time as u32) & 0xF) << 21 | ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17 | ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14 | ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10 | ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8 | ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6 | ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4 | ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
self.assert_register(0x82, Mct8316AVData::Four(expected_value)) self.assert_register(0x82, Mct8316AVData::Four(expected_value))
} }
pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result<Self> { pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_motor_startup2(motor_startup2: {motor_startup2:?})"); trace!(
"Mct8316AVEeprom::set_motor_startup2(self: {self:?}, motor_startup2: {motor_startup2:?})"
);
let expected_value = let expected_value = ((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30
((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30 | ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27
| ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27 | ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23
| ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23 | ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18 | ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13 | ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8 | if motor_startup2.auto_handoff_enable {
| if motor_startup2.auto_handoff_enable { 1u32 << 7 } else { 0u32 } 1u32 << 7
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6 } else {
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2; 0u32
}
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2;
self.assert_register(0x84, Mct8316AVData::Four(expected_value)) self.assert_register(0x84, Mct8316AVData::Four(expected_value))
} }
// The casts in here are save due to clamps
#[allow(clippy::cast_possible_truncation)]
#[allow(clippy::cast_sign_loss)]
pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result<Self> { pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop1(closed_loop1: {closed_loop1:?})"); trace!("Mct8316AVEeprom::set_closed_loop1(self: {self:?}, closed_loop1: {closed_loop1:?})");
let lead_angle = (closed_loop1.lead_angle / 0.12f32).round().clamp(0.0f32, u8::MAX as f32) as u8; let lead_angle = (closed_loop1.lead_angle / 0.12f32)
.round()
.clamp(0.0f32, f32::from(u8::MAX)) as u8;
let expected_value = let expected_value = ((closed_loop1.commutation_mode as u32) & 0x2) << 29
((closed_loop1.commutation_mode as u32) & 0x2) << 29 | ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24
| ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24 | ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23
| ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23 | ((closed_loop1.closed_loop_deceleration_rate as u32) & 0x1F) << 18
| ((closed_loop1.closed_loop_deceleration_rate as u32) & 0x1F) << 18 | ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13 | ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11 | ((closed_loop1.pwm_mode as u32) & 0x1) << 10
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10 | ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9 | u32::from(lead_angle) << 1;
| (lead_angle as u32) << 1;
self.assert_register(0x86, Mct8316AVData::Four(expected_value)) self.assert_register(0x86, Mct8316AVData::Four(expected_value))
} }
pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> { pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop2(closed_loop2: {closed_loop2:?})"); trace!("Mct8316AVEeprom::set_closed_loop2(self: {self:?}, closed_loop2: {closed_loop2:?})");
let expected_value = let expected_value = ((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29 | ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25 | ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24 | ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21 | ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18 | ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14 | ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11 | ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8 | if closed_loop2.avs_enable {
| if closed_loop2.avs_enable { 1u32 << 7 } else { 0u32 } 1u32 << 7
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3; } else {
0u32
}
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
self.assert_register(0x88, Mct8316AVData::Four(expected_value)) self.assert_register(0x88, Mct8316AVData::Four(expected_value))
} }
pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> { pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop3(closed_loop3: {closed_loop3:?})"); trace!("Mct8316AVEeprom::set_closed_loop3(self: {self:?}, closed_loop3: {closed_loop3:?})");
let expected_value = let expected_value = ((closed_loop3.degauss_samples as u32) & 0x2) << 29
((closed_loop3.degauss_samples as u32) & 0x2) << 29 | ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27 | ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25 | ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23 | ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21
| ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21 | ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19
| ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19 | ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17
| ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17 | ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11 | ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5 | ((closed_loop3.commutation_method as u32) & 0x1) << 4
| ((closed_loop3.commutation_method as u32) & 0x1) << 4 | ((closed_loop3.degauss_window as u32) & 0x7) << 1
| ((closed_loop3.degauss_window as u32) & 0x7) << 1 | u32::from(closed_loop3.degauss_enable);
| if closed_loop3.degauss_enable { 1 } else { 0u32 };
self.assert_register(0x8A, Mct8316AVData::Four(expected_value)) self.assert_register(0x8A, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> { pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop4(closed_loop4: {closed_loop4:?})"); trace!("Mct8316AVEeprom::set_closed_loop4(self: {self:?}, closed_loop4: {closed_loop4:?})");
let expected_value = let expected_value = if closed_loop4.wcomp_blanking_enable {
if closed_loop4.wcomp_blanking_enable { 1u32 << 19 } else { 0u32 } 1u32 << 19
| ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16 } else {
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13 0u32
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9 } | ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
| if closed_loop4.dynamic_braking_current_enable { 1u32 << 8 } else { 0u32 } | ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
| if closed_loop4.fast_deceleration_enable { 1u32 << 7 } else { 0u32 } | ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
| ((closed_loop4.fast_deceleration_current_threshold as u32) & 0xF) << 3 | if closed_loop4.dynamic_braking_current_enable {
| ((closed_loop4.fast_brake_delta as u32) & 0x7) << 0; 1u32 << 8
} else {
0u32
}
| if closed_loop4.fast_deceleration_enable {
1u32 << 7
} else {
0u32
}
| ((closed_loop4.fast_deceleration_current_threshold as u32) & 0xF) << 3
| ((closed_loop4.fast_brake_delta as u32) & 0x7) << 0;
self.assert_register(0x8C, Mct8316AVData::Four(expected_value)) self.assert_register(0x8C, Mct8316AVData::Four(expected_value))
} }
// The casts in here are save due to clamps
#[allow(clippy::cast_possible_truncation)]
#[allow(clippy::cast_sign_loss)]
// These names come from the ICD
#[allow(clippy::similar_names)]
pub fn set_constant_speed(self, constant_speed: ConstantSpeed) -> Result<Self> { pub fn set_constant_speed(self, constant_speed: ConstantSpeed) -> Result<Self> {
trace!("Mct8316AVEeprom::set_constant_speed(constant_speed: {constant_speed:?})"); trace!(
"Mct8316AVEeprom::set_constant_speed(self: {self:?}, constant_speed: {constant_speed:?})"
);
let speed_power_kp = (constant_speed.speed_power_kp * 10000f64).round().clamp(0.0f64, 0x3FF as f64) as u32; let speed_power_kp = (constant_speed.speed_power_kp * 10000f64)
let speed_power_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32; .round()
.clamp(0.0f64, f64::from(0x3FF)) as u32;
let speed_power_ki = (constant_speed.speed_power_ki * 1_000_000_f64)
.round()
.clamp(0.0f64, f64::from(0xFFF)) as u32;
let expected_value = let expected_value = speed_power_kp << 20
speed_power_kp << 20 | speed_power_ki << 8
| speed_power_ki << 8 | ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5 | ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2 | ((constant_speed.mode as u32) & 0x3) << 9;
| ((constant_speed.mode as u32) & 0x3) << 9;
self.assert_register(0x8E, Mct8316AVData::Four(expected_value)) self.assert_register(0x8E, Mct8316AVData::Four(expected_value))
} }
// The casts in here are save due to clamps
#[allow(clippy::cast_possible_truncation)]
#[allow(clippy::cast_sign_loss)]
// To follow a pattern for the ICD
#[allow(clippy::identity_op)]
pub fn set_constant_power(self, constant_power: ConstantPower) -> Result<Self> { pub fn set_constant_power(self, constant_power: ConstantPower) -> Result<Self> {
trace!("Mct8316AVEeprom::set_constant_power(constant_power: {constant_power:?})"); trace!(
"Mct8316AVEeprom::set_constant_power(self: {self:?}, constant_power: {constant_power:?})"
);
let max_speed = (constant_power.max_speed * 16f64).round().clamp(0.0f64, 0xFFFF as f64) as u32; let max_speed = (constant_power.max_speed * 16f64)
let max_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32; .round()
.clamp(0.0f64, f64::from(0xFFFF)) as u32;
let max_power = (constant_power.max_power * 4f64)
.round()
.clamp(0.0f64, f64::from(0x3FF)) as u32;
let expected_value = let expected_value = max_speed << 15
max_speed << 15 | if constant_power.dead_time_compensation_enable {
| if constant_power.dead_time_compensation_enable { 1u32 << 14 } else { 0u32 } 1u32 << 14
| max_power << 4 } else {
| ((constant_power.power_hysteresis as u32) & 0x3) << 2 0u32
| ((constant_power.mode as u32) & 0x3) << 0; }
| max_power << 4
| ((constant_power.power_hysteresis as u32) & 0x3) << 2
| ((constant_power.mode as u32) & 0x3) << 0;
self.assert_register(0x90, Mct8316AVData::Four(expected_value)) self.assert_register(0x90, Mct8316AVData::Four(expected_value))
} }
pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> { pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> {
trace!("Mct8316AVEeprom::set_two_phase_profile(profile: {profile:?})"); trace!("Mct8316AVEeprom::set_two_phase_profile(self: {self:?}, profile: {profile:?})");
let expected_value = let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
((profile.steps[0] as u32) & 0x7) << 28 | ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[1] as u32) & 0x7) << 25 | ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[2] as u32) & 0x7) << 22 | ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[3] as u32) & 0x7) << 19 | ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[4] as u32) & 0x7) << 16 | ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[5] as u32) & 0x7) << 13 | ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[6] as u32) & 0x7) << 10 | ((profile.steps[7] as u32) & 0x7) << 7;
| ((profile.steps[7] as u32) & 0x7) << 7;
self.assert_register(0x96, Mct8316AVData::Four(expected_value)) self.assert_register(0x96, Mct8316AVData::Four(expected_value))
} }
pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> { pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> {
trace!("Mct8316AVEeprom::set_three_phase_profile(profile: {profile:?})"); trace!("Mct8316AVEeprom::set_three_phase_profile(self: {self:?}, profile: {profile:?})");
let expected_value = let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
((profile.steps[0] as u32) & 0x7) << 28 | ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[1] as u32) & 0x7) << 25 | ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[2] as u32) & 0x7) << 22 | ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[3] as u32) & 0x7) << 19 | ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[4] as u32) & 0x7) << 16 | ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[5] as u32) & 0x7) << 13 | ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[6] as u32) & 0x7) << 10 | ((profile.steps[7] as u32) & 0x7) << 7
| ((profile.steps[7] as u32) & 0x7) << 7 | ((profile.lead_angle as u32) & 0x3) << 5;
| ((profile.lead_angle as u32) & 0x3) << 5;
self.assert_register(0x98, Mct8316AVData::Four(expected_value)) self.assert_register(0x98, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> { pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_trap_config1(trap_config1: {trap_config1:?})"); trace!("Mct8316AVEeprom::set_trap_config1(self: {self:?}, trap_config1: {trap_config1:?})");
let expected_value = let expected_value = ((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22 | ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16 | ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15 | ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10 | ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7 | ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2 | ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0;
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0
;
self.assert_register(0x9A, Mct8316AVData::Four(expected_value)) self.assert_register(0x9A, Mct8316AVData::Four(expected_value))
} }
pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> { pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_trap_config2(trap_config2: {trap_config2:?})"); trace!("Mct8316AVEeprom::set_trap_config2(self: {self:?}, trap_config2: {trap_config2:?})");
let expected_value = let expected_value = (u32::from(trap_config2.blanking_time_microseconds) & 0xF) << 27
((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27 | (u32::from(trap_config2.comparator_deglitch_time_microseconds) & 0x7) << 24
| ((trap_config2.comparator_deglitch_time_microseconds as u32) & 0x7) << 24 | ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
self.assert_register(0x9C, Mct8316AVData::Four(expected_value)) self.assert_register(0x9C, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_fault_config1(self, fault_config1: FaultConfig1) -> Result<Self> { pub fn set_fault_config1(self, fault_config1: FaultConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_fault_config1(fault_config1: {fault_config1:?})"); trace!(
"Mct8316AVEeprom::set_fault_config1(self: {self:?}, fault_config1: {fault_config1:?})"
);
let expected_value = let expected_value = ((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27
((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27 | ((fault_config1.cycle_by_cycle_current_limit as u32) & 0xF) << 23
| ((fault_config1.cycle_by_cycle_current_limit as u32) & 0xF) << 23 | ((fault_config1.lock_detection_current_limit as u32) & 0xF) << 19
| ((fault_config1.lock_detection_current_limit as u32) & 0xF) << 19 | ((fault_config1.lock_detection_current_limit_mode as u32) & 0xF) << 15
| ((fault_config1.lock_detection_current_limit_mode as u32) & 0xF) << 15 | ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11
| ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11 | (u32::from(fault_config1.cycle_by_cycle_pwm_limit) & 0x7) << 8
| ((fault_config1.cycle_by_cycle_pwm_limit as u32) & 0x7) << 8 | ((fault_config1.motor_lock_mode as u32) & 0xF) << 3
| ((fault_config1.motor_lock_mode as u32) & 0xF) << 3 | ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
| ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
self.assert_register(0x92, Mct8316AVData::Four(expected_value)) self.assert_register(0x92, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_fault_config2(self, fault_config2: FaultConfig2) -> Result<Self> { pub fn set_fault_config2(self, fault_config2: FaultConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_fault_config2(fault_config2: {fault_config2:?})"); trace!(
"Mct8316AVEeprom::set_fault_config2(self: {self:?}, fault_config2: {fault_config2:?})"
);
let expected_value = let expected_value = if fault_config2.lock_abnormal_speed_enable {
if fault_config2.lock_abnormal_speed_enable { 1u32 << 30 } else { 0u32 } 1u32 << 30
| if fault_config2.lock_loss_of_sync_enable { 1u32 << 29 } else { 0u32 } } else {
| if fault_config2.lock_no_motor_enable { 1u32 << 28 } else { 0u32 } 0u32
| ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24 } | if fault_config2.lock_loss_of_sync_enable {
| ((fault_config2.loss_sync_times as u32) & 0x7) << 21 1u32 << 29
| ((fault_config2.no_motor_threshold as u32) & 0x7) << 18 } else {
| ((fault_config2.max_motor_voltage_mode as u32) & 0x1) << 17 0u32
| ((fault_config2.max_motor_voltage as u32) & 0x7) << 14 } | if fault_config2.lock_no_motor_enable {
| ((fault_config2.min_motor_voltage_mode as u32) & 0x1) << 13 1u32 << 28
| ((fault_config2.min_motor_voltage as u32) & 0x7) << 10 } else {
| ((fault_config2.automatic_retries as u32) & 0x7) << 7 0u32
| ((fault_config2.lock_min_speed as u32) & 0x7) << 4 } | ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24
| ((fault_config2.abnormal_speed_lock as u32) & 0x3) << 2 | ((fault_config2.loss_sync_times as u32) & 0x7) << 21
| ((fault_config2.zero_duty_threshold as u32) & 0x3) << 0; | ((fault_config2.no_motor_threshold as u32) & 0x7) << 18
| ((fault_config2.max_motor_voltage_mode as u32) & 0x1) << 17
| ((fault_config2.max_motor_voltage as u32) & 0x7) << 14
| ((fault_config2.min_motor_voltage_mode as u32) & 0x1) << 13
| ((fault_config2.min_motor_voltage as u32) & 0x7) << 10
| ((fault_config2.automatic_retries as u32) & 0x7) << 7
| ((fault_config2.lock_min_speed as u32) & 0x7) << 4
| ((fault_config2.abnormal_speed_lock as u32) & 0x3) << 2
| ((fault_config2.zero_duty_threshold as u32) & 0x3) << 0;
self.assert_register(0x94, Mct8316AVData::Four(expected_value)) self.assert_register(0x94, Mct8316AVData::Four(expected_value))
} }
pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result<Self> { pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_pin_config1(pin_config1: {pin_config1:?})"); trace!("Mct8316AVEeprom::set_pin_config1(self: {self:?}, pin_config1: {pin_config1:?})");
let expected_value = let expected_value = (u32::from(pin_config1.dacout1_address) & 0xFFF) << 19
((pin_config1.dacout1_address as u32) & 0xFFF) << 19 | (u32::from(pin_config1.dacout2_address) & 0xFFF) << 7
| ((pin_config1.dacout2_address as u32) & 0xFFF) << 7 | ((pin_config1.brake_input_config as u32) & 0x3) << 5
| ((pin_config1.brake_input_config as u32) & 0x3) << 5 | ((pin_config1.direction_input_config as u32) & 0x3) << 3
| ((pin_config1.direction_input_config as u32) & 0x3) << 3 | ((pin_config1.speed_input_config as u32) & 0x3) << 1;
| ((pin_config1.speed_input_config as u32) & 0x3) << 1;
self.assert_register(0xA4, Mct8316AVData::Four(expected_value)) self.assert_register(0xA4, Mct8316AVData::Four(expected_value))
} }
pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result<Self> { pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_pin_config2(pin_config2: {pin_config2:?})"); trace!("Mct8316AVEeprom::set_pin_config2(self: {self:?}, pin_config2: {pin_config2:?})");
let expected_value = let expected_value = ((pin_config2.pin36config as u32) & 0x3) << 29
((pin_config2.pin36config as u32) & 0x3) << 29 | ((pin_config2.pin37_38config as u32) & 0x1) << 27
| ((pin_config2.pin37_38config as u32) & 0x1) << 27 | ((pin_config2.sleep_time as u32) & 0x3) << 18
| ((pin_config2.sleep_time as u32) & 0x3) << 18 | if pin_config2.enable_external_watchdog {
| if pin_config2.enable_external_watchdog { 1u32 << 17 } else { 0u32 } 1u32 << 17
| ((pin_config2.external_watchdog_source as u32) & 0x1) << 16 } else {
| ((pin_config2.external_watchdog_fault_mode as u32) & 0x1) << 15 0u32
| ((pin_config2.external_watchdog_frequency as u32) & 0x3) << 13; }
| ((pin_config2.external_watchdog_source as u32) & 0x1) << 16
| ((pin_config2.external_watchdog_fault_mode as u32) & 0x1) << 15
| ((pin_config2.external_watchdog_frequency as u32) & 0x3) << 13;
self.assert_register(0xA6, Mct8316AVData::Four(expected_value)) self.assert_register(0xA6, Mct8316AVData::Four(expected_value))
} }
pub fn set_device_config(self, device_config: DeviceConfig) -> Result<Self> { pub fn set_device_config(self, device_config: DeviceConfig) -> Result<Self> {
trace!("Mct8316AVEeprom::set_device_config(device_config: {device_config:?})"); trace!(
"Mct8316AVEeprom::set_device_config(self: {self:?}, device_config: {device_config:?})"
);
let expected_value = let expected_value = (u32::from(device_config.max_frequency) & 0x7FFF) << 16
((device_config.max_frequency as u32) & 0x7FFF) << 16 | if device_config.stl_enable {
| if device_config.stl_enable { 1u32 << 15 } else { 0u32 } 1u32 << 15
| if device_config.ssm_enable { 1u32 << 14 } else { 0u32 } } else {
| ((device_config.device_mode as u32) & 0x1) << 11 0u32
| ((device_config.pwm_range_select as u32) & 0x1) << 10 }
| ((device_config.clock_source as u32) & 0x3) << 8 | if device_config.ssm_enable {
| if device_config.external_clock_enable { 1u32 << 6 } else { 0u32 } 1u32 << 14
| ((device_config.external_clock_frequency as u32) & 0x7) << 3; } else {
0u32
}
| ((device_config.device_mode as u32) & 0x1) << 11
| ((device_config.pwm_range_select as u32) & 0x1) << 10
| ((device_config.clock_source as u32) & 0x3) << 8
| if device_config.external_clock_enable {
1u32 << 6
} else {
0u32
}
| ((device_config.external_clock_frequency as u32) & 0x7) << 3;
self.assert_register(0xA8, Mct8316AVData::Four(expected_value)) self.assert_register(0xA8, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_gate_driver_config1(self, gate_driver_config1: GateDriverConfig1) -> Result<Self> { pub fn set_gate_driver_config1(self, gate_driver_config1: GateDriverConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_gate_driver_config1(gate_driver_config1: {gate_driver_config1:?})"); trace!(
"Mct8316AVEeprom::set_gate_driver_config1(self: {self:?}, gate_driver_config1: {gate_driver_config1:?})"
);
let expected_value = let expected_value = ((gate_driver_config1.slew_rate as u32) & 0x3) << 26
((gate_driver_config1.slew_rate as u32) & 0x3) << 26 | ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19
| ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19 | if gate_driver_config1.overvoltage_protection_enable {
| if gate_driver_config1.overvoltage_protection_enable { 1u32 << 18 } else { 0u32 } 1u32 << 18
| if gate_driver_config1.overtemperature_warning_enable { 1u32 << 16 } else { 0u32 } } else {
| ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12 0u32
| ((gate_driver_config1.overcurrent_protection_retry_time as u32) & 0x1) << 11 }
| ((gate_driver_config1.overcurrent_protection_level as u32) & 0x1) << 10 | if gate_driver_config1.overtemperature_warning_enable {
| ((gate_driver_config1.overcurrent_fault_mode as u32) & 0x3) << 8 1u32 << 16
| ((gate_driver_config1.low_demag_comparator_threshold as u32) & 0x1) << 5 } else {
| ((gate_driver_config1.high_demag_comparator_threshold as u32) & 0x1) << 4 0u32
| if gate_driver_config1.synchronous_rectification_enable { 1u32 << 3 } else { 0u32 } }
| if gate_driver_config1.asynchronous_rectification_enable { 1u32 << 2 } else { 0u32 } | ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12
| ((gate_driver_config1.current_sense_amplifier as u32) & 0x3) << 0; | ((gate_driver_config1.overcurrent_protection_retry_time as u32) & 0x1) << 11
| ((gate_driver_config1.overcurrent_protection_level as u32) & 0x1) << 10
| ((gate_driver_config1.overcurrent_fault_mode as u32) & 0x3) << 8
| ((gate_driver_config1.low_demag_comparator_threshold as u32) & 0x1) << 5
| ((gate_driver_config1.high_demag_comparator_threshold as u32) & 0x1) << 4
| if gate_driver_config1.synchronous_rectification_enable {
1u32 << 3
} else {
0u32
}
| if gate_driver_config1.asynchronous_rectification_enable {
1u32 << 2
} else {
0u32
}
| ((gate_driver_config1.current_sense_amplifier as u32) & 0x3) << 0;
self.assert_register(0xAC, Mct8316AVData::Four(expected_value)) self.assert_register(0xAC, Mct8316AVData::Four(expected_value))
} }
pub fn set_gate_driver_config2(self, gate_driver_config2: GateDriverConfig2) -> Result<Self> { pub fn set_gate_driver_config2(self, gate_driver_config2: GateDriverConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_gate_driver_config2(gate_driver_config2: {gate_driver_config2:?})"); trace!(
"Mct8316AVEeprom::set_gate_driver_config2(self: {self:?}, gate_driver_config2: {gate_driver_config2:?})"
);
let expected_value = let expected_value = if gate_driver_config2.driver_delay_compensation_enable {
if gate_driver_config2.driver_delay_compensation_enable { 1u32 << 30 } else { 0u32 } 1u32 << 30
| ((gate_driver_config2.target_delay as u32) & 0xF) << 26 } else {
| ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25 0u32
| if gate_driver_config2.buck_power_sequencing_disable { 1u32 << 24 } else { 0u32 } } | ((gate_driver_config2.target_delay as u32) & 0xF) << 26
| ((gate_driver_config2.buck_current_limit as u32) & 0x1) << 23 | ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25
| ((gate_driver_config2.buck_voltage_selection as u32) & 0x3) << 21 | if gate_driver_config2.buck_power_sequencing_disable {
| if gate_driver_config2.buck_disable { 1u32 << 20 } else { 0u32 }; 1u32 << 24
} else {
0u32
}
| ((gate_driver_config2.buck_current_limit as u32) & 0x1) << 23
| ((gate_driver_config2.buck_voltage_selection as u32) & 0x3) << 21
| if gate_driver_config2.buck_disable {
1u32 << 20
} else {
0u32
};
self.assert_register(0xAE, Mct8316AVData::Four(expected_value)) self.assert_register(0xAE, Mct8316AVData::Four(expected_value))
} }
fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> { fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> {
trace!("Mct8316AVEeprom::assert_register(address: {address:06x}, value: {value})"); trace!(
"Mct8316AVEeprom::assert_register(self: {self:?}, address: {address:06x}, value: {value})"
);
let mut read_value = value.clone(); let mut read_value = value;
self.driver.read(address, &mut read_value)?; self.driver.read(address, &mut read_value)?;
@@ -412,12 +560,13 @@ where
} }
pub fn commit(self) -> Result<()> { pub fn commit(self) -> Result<()> {
trace!("Mct8316AVEeprom::commit()"); trace!("Mct8316AVEeprom::commit(self: {self:?})");
if self.modified { if self.modified {
self.driver.write(0x0000E6, Mct8316AVData::Four(0x80000000))?; self.driver
.write(0x00_00E6, Mct8316AVData::Four(0x8000_0000))?;
// Wait for EEPROM operation to complete // Wait for EEPROM operation to complete
sleep(Duration::from_millis(100)); sleep(Duration::from_millis(100));
} }
Ok(()) Ok(())
} }
} }

6
flight/src/hardware/mct8316a/fault_config.rs
View File

@@ -17,10 +17,12 @@ impl Default for FaultConfig1 {
fn default() -> Self { fn default() -> Self {
Self { Self {
no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds1, no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds1,
cycle_by_cycle_current_limit: CycleByCycleCurrentLimit::RecoverNextPwmFaultActiveRecirculation, cycle_by_cycle_current_limit:
CycleByCycleCurrentLimit::RecoverNextPwmFaultActiveRecirculation,
lock_detection_current_limit: LockDetectionCurrentLimit::NotApplicable, lock_detection_current_limit: LockDetectionCurrentLimit::NotApplicable,
lock_detection_current_limit_mode: LockMode::LatchFaultTristated, lock_detection_current_limit_mode: LockMode::LatchFaultTristated,
lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime::Milliseconds1, lock_detection_current_limit_deglitch_time:
LockDetectionCurrentLimitDeglitchTime::Milliseconds1,
cycle_by_cycle_pwm_limit: 0, cycle_by_cycle_pwm_limit: 0,
motor_lock_mode: LockMode::LatchFaultTristated, motor_lock_mode: LockMode::LatchFaultTristated,
lock_retry_time: LockRetryTime::Milliseconds100, lock_retry_time: LockRetryTime::Milliseconds100,

2
flight/src/hardware/mct8316a/gate_driver_config.rs
View File

@@ -1,5 +1,7 @@
#![allow(dead_code)] #![allow(dead_code)]
// This comes from the ICD
#[allow(clippy::struct_excessive_bools)]
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct GateDriverConfig1 { pub struct GateDriverConfig1 {
pub slew_rate: SlewRate, pub slew_rate: SlewRate,

2
flight/src/hardware/mct8316a/isd_config.rs
View File

@@ -1,5 +1,7 @@
#![allow(dead_code)] #![allow(dead_code)]
// This comes from the ICD
#[allow(clippy::struct_excessive_bools)]
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct IsdConfig { pub struct IsdConfig {
pub enable_isd: bool, pub enable_isd: bool,

14
flight/src/hardware/mct8316a/mod.rs
View File

@@ -1,16 +1,16 @@
mod closed_loop;
mod constant_power;
mod constant_speed;
mod device_config;
mod driver; mod driver;
mod eeprom; mod eeprom;
mod fault_config;
mod gate_driver_config;
mod isd_config; mod isd_config;
mod motor_startup; mod motor_startup;
mod closed_loop;
mod constant_speed;
mod constant_power;
mod phase_profile; mod phase_profile;
mod trap_config;
mod fault_config;
mod pin_config; mod pin_config;
mod device_config; mod trap_config;
mod gate_driver_config;
use anyhow::Result; use anyhow::Result;

6
flight/src/hardware/mct8316a/motor_startup.rs
View File

@@ -35,7 +35,7 @@ impl Default for MotorStartup1 {
pub enum MotorStartupMethod { pub enum MotorStartupMethod {
Align = 0x0, Align = 0x0,
DoubleAlign = 0x1, DoubleAlign = 0x1,
IPD = 0x2, Ipd = 0x2,
SlowFirstCycle = 0x3, SlowFirstCycle = 0x3,
} }
@@ -200,7 +200,6 @@ impl Default for MotorStartup2 {
} }
} }
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub enum OpenLoopCurrentLimitMode { pub enum OpenLoopCurrentLimitMode {
OpenLoopCurrentLimit = 0x0, OpenLoopCurrentLimit = 0x0,
@@ -358,6 +357,3 @@ pub enum MinimumDutyCycle {
Percent25 = 0xE, Percent25 = 0xE,
Percent30 = 0xF, Percent30 = 0xF,
} }

1
flight/src/hardware/mct8316a/trap_config.rs
View File

@@ -135,4 +135,3 @@ impl Default for TrapConfig2 {
} }
} }
} }

30
flight/src/hardware/mod.rs
View File

@@ -2,12 +2,17 @@ use crate::hardware::mcp23017::Mcp23017;
use crate::hardware::mct8316a::Mct8316a; use crate::hardware::mct8316a::Mct8316a;
use anyhow::Result; use anyhow::Result;
use embedded_hal::pwm::SetDutyCycle; use embedded_hal::pwm::SetDutyCycle;
use log::trace;
use std::fmt::Debug;
pub trait Hardware { pub trait Hardware {
type Mcp23017<'a>: Mcp23017 + Send + Debug
where
Self: 'a;
type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync; type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync;
fn new_mcp23017_a(&self) -> Result<impl Mcp23017 + Sync>; fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>>;
fn new_mcp23017_b(&self) -> Result<impl Mcp23017 + Sync>; fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>>;
fn new_pwm0(&self) -> Result<Self::Pwm>; fn new_pwm0(&self) -> Result<Self::Pwm>;
@@ -21,24 +26,23 @@ mod raspi;
#[cfg(feature = "raspi")] #[cfg(feature = "raspi")]
pub fn initialize() -> Result<impl Hardware> { pub fn initialize() -> Result<impl Hardware> {
trace!("initialize()");
raspi::RaspiHardware::new() raspi::RaspiHardware::new()
} }
#[cfg(not(feature = "raspi"))] #[cfg(not(feature = "raspi"))]
#[allow(unreachable_code)] #[allow(unreachable_code)]
pub fn initialize() -> Result<impl Hardware> { pub fn initialize() -> Result<impl Hardware> {
panic!("Can not Initialize"); trace!("initialize()");
sim::SimHardware::new()
Ok(())
} }
#[cfg(not(feature = "raspi"))] pub mod error;
mod bno085;
#[cfg(not(feature = "raspi"))]
mod imu;
mod error;
pub mod mcp23017;
mod mcp3208;
pub mod channelization; pub mod channelization;
pub(crate) mod mct8316a; pub mod mcp23017;
#[cfg(feature = "raspi")]
mod mcp3208;
pub mod mct8316a;
pub mod pin;
mod sim;

26
flight/src/hardware/pin.rs Normal file
View File

@@ -0,0 +1,26 @@
use embedded_hal::digital::PinState;
use nautilus_common::command::{SetPin, ValidPriorityCommand};
use std::time::Instant;
pub trait PinDevice {
fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8);
fn new_set_pin_callback<'a>(&self) -> impl Fn(ValidPriorityCommand<SetPin>) + 'a
where
Self: Sized + Clone + 'a,
{
let this = self.clone();
move |cmd| {
this.set_pin(
cmd.pin,
cmd.value.into(),
cmd.get_valid_until_instant(),
cmd.priority,
);
}
}
}
pub trait Pin {
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8);
}

56
flight/src/hardware/raspi/mod.rs
View File

@@ -1,10 +1,11 @@
mod pwm; mod pwm;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017Driver}; use crate::hardware::Hardware;
use crate::hardware::mcp3208::Mcp3208; use crate::hardware::mcp3208::Mcp3208;
use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a}; use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
use crate::hardware::raspi::pwm::PwmWrapper; use crate::hardware::raspi::pwm::PwmWrapper;
use crate::hardware::Hardware; use crate::hardware::sim::mct8316a::SimMct8316a;
use anyhow::Result; use anyhow::Result;
use embedded_hal_bus::i2c::MutexDevice; use embedded_hal_bus::i2c::MutexDevice;
use log::{debug, info, trace}; use log::{debug, info, trace};
@@ -18,10 +19,12 @@ use std::sync::Mutex;
const CLOCK_1MHZ: u32 = 1_000_000; const CLOCK_1MHZ: u32 = 1_000_000;
#[derive(Debug)]
pub struct RaspiHardware { pub struct RaspiHardware {
_gpio: Gpio, _gpio: Gpio,
i2c_bus: Mutex<I2c>, i2c_bus: Mutex<I2c>,
mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>, mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
mct8316a: Mutex<SimMct8316a>,
} }
impl RaspiHardware { impl RaspiHardware {
@@ -35,31 +38,44 @@ impl RaspiHardware {
Ok(Self { Ok(Self {
_gpio: Gpio::new()?, _gpio: Gpio::new()?,
i2c_bus: Mutex::new(I2c::with_bus(0u8)?), i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new( mcp3208: Mcp3208::new(
Bus::Spi1, SimpleHalSpiDevice::new(Spi::new(
SlaveSelect::Ss1, Bus::Spi1,
CLOCK_1MHZ, SlaveSelect::Ss1,
Mode::Mode0, CLOCK_1MHZ,
)?), 3.3f64).into(), Mode::Mode0,
)?),
3.3f64,
)
.into(),
mct8316a: SimMct8316a::new().into(),
// mct8316a: SimMct8316a::new().into(),
}) })
} }
} }
impl Hardware for RaspiHardware { impl Hardware for RaspiHardware {
type Mcp23017<'a> = Mcp23017Driver<MutexDevice<'a, I2c>>;
type Pwm = PwmWrapper; type Pwm = PwmWrapper;
fn new_mcp23017_a(&self) -> Result<impl Mcp23017> { fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
trace!("RaspiHardware::new_mcp23017_a()"); trace!("RaspiHardware::new_mcp23017_a(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000)) Ok(Mcp23017Driver::new(
MutexDevice::new(&self.i2c_bus),
0b0100000,
))
} }
fn new_mcp23017_b(&self) -> Result<impl Mcp23017> { fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
trace!("RaspiHardware::new_mcp23017_b()"); trace!("RaspiHardware::new_mcp23017_b(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001)) Ok(Mcp23017Driver::new(
MutexDevice::new(&self.i2c_bus),
0b0100001,
))
} }
fn new_pwm0(&self) -> Result<Self::Pwm> { fn new_pwm0(&self) -> Result<Self::Pwm> {
trace!("RaspiHardware::new_pwm0()"); trace!("RaspiHardware::new_pwm0(self: {self:?})");
// Unfortunately the current version of rpi_pal assumes an older version // Unfortunately the current version of rpi_pal assumes an older version
// of the kernel where pwmchip for RPi5 was 2 // of the kernel where pwmchip for RPi5 was 2
const PWMCHIP: u8 = 0; const PWMCHIP: u8 = 0;
@@ -68,13 +84,15 @@ impl Hardware for RaspiHardware {
} }
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> { fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
trace!("RaspiHardware::new_mct8316a()"); trace!("RaspiHardware::new_mct8316a(self: {self:?})");
Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000)) Ok(Mct8316AVDriver::new(
MutexDevice::new(&self.mct8316a),
0b0000000,
))
} }
fn get_battery_voltage(&self) -> Result<f64> { fn get_battery_voltage(&self) -> Result<f64> {
trace!("RaspiHardware::get_battery_voltage()"); trace!("RaspiHardware::get_battery_voltage(self: {self:?})");
self.mcp3208.borrow_mut().read_single(1) self.mcp3208.borrow_mut().read_single(1)
} }
} }

36
flight/src/hardware/raspi/pwm.rs
View File

@@ -1,11 +1,12 @@
use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle}; use crate::hardware::error::WrappingError;
use embedded_hal::pwm::{ErrorType, SetDutyCycle};
use log::trace; use log::trace;
use rpi_pal::pwm::Pwm; use rpi_pal::pwm::Pwm;
use std::fmt::{Display, Formatter};
use std::time::Duration; use std::time::Duration;
const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
#[derive(Debug)]
pub struct PwmWrapper { pub struct PwmWrapper {
pwm: Pwm, pwm: Pwm,
} }
@@ -16,41 +17,24 @@ impl PwmWrapper {
pwm.set_period(PWM_PERIOD)?; pwm.set_period(PWM_PERIOD)?;
pwm.enable()?; pwm.enable()?;
pwm.set_reset_on_drop(true); pwm.set_reset_on_drop(true);
Ok(Self { Ok(Self { pwm })
pwm
})
} }
} }
#[derive(Debug)]
pub struct ErrorWrapper(rpi_pal::pwm::Error);
impl embedded_hal::pwm::Error for ErrorWrapper {
fn kind(&self) -> ErrorKind {
ErrorKind::Other
}
}
impl Display for ErrorWrapper {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
self.0.fmt(f)
}
}
impl std::error::Error for ErrorWrapper {}
impl ErrorType for PwmWrapper { impl ErrorType for PwmWrapper {
type Error = ErrorWrapper; type Error = WrappingError<rpi_pal::pwm::Error>;
} }
impl SetDutyCycle for PwmWrapper { impl SetDutyCycle for PwmWrapper {
fn max_duty_cycle(&self) -> u16 { fn max_duty_cycle(&self) -> u16 {
trace!("PwmWrapper::max_duty_cycle()"); trace!("PwmWrapper::max_duty_cycle(self: {self:?})");
u16::MAX u16::MAX
} }
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
trace!("PwmWrapper::set_duty_cycle(duty: {duty})"); trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty: {duty})");
self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(ErrorWrapper) self.pwm
.set_duty_cycle((duty as f64) / (u16::MAX as f64))
.map_err(WrappingError)
} }
} }

71
flight/src/hardware/sim/hardware.rs Normal file
View File

@@ -0,0 +1,71 @@
use crate::hardware::Hardware;
use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
use crate::hardware::sim::mcp23017::SimMcp23017;
use crate::hardware::sim::mct8316a::SimMct8316a;
use crate::hardware::sim::pwm::SimPwm;
use anyhow::Result;
use embedded_hal_bus::i2c::MutexDevice;
use log::trace;
use std::sync::Mutex;
#[derive(Debug)]
pub struct SimHardware {
mcp23017a: Mutex<SimMcp23017>,
mcp23017b: Mutex<SimMcp23017>,
mct8316a: Mutex<SimMct8316a>,
battery_voltage: f64,
}
impl SimHardware {
// This returns a Result to match the raspi implementation
#[allow(clippy::unnecessary_wraps)]
pub fn new() -> Result<Self> {
trace!("SimHardware::new()");
Ok(Self {
mcp23017a: SimMcp23017::new().into(),
mcp23017b: SimMcp23017::new().into(),
mct8316a: SimMct8316a::new().into(),
battery_voltage: 12.4f64,
})
}
}
impl Hardware for SimHardware {
type Mcp23017<'a> = Mcp23017Driver<MutexDevice<'a, SimMcp23017>>;
type Pwm = SimPwm;
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
trace!("SimHardware::new_mcp23017_a(self: {self:?})");
Ok(Mcp23017Driver::new(
MutexDevice::new(&self.mcp23017a),
0b010_0000,
))
}
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
trace!("SimHardware::new_mcp23017_b(self: {self:?})");
Ok(Mcp23017Driver::new(
MutexDevice::new(&self.mcp23017b),
0b010_0001,
))
}
fn new_pwm0(&self) -> Result<Self::Pwm> {
trace!("SimHardware::new_pwm0(self: {self:?})");
Ok(SimPwm::new())
}
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
trace!("SimHardware::new_mct8316a(self: {self:?})");
Ok(Mct8316AVDriver::new(
MutexDevice::new(&self.mct8316a),
0b000_0000,
))
}
fn get_battery_voltage(&self) -> Result<f64> {
trace!("SimHardware::get_battery_voltage(self: {self:?})");
Ok(self.battery_voltage)
}
}

57
flight/src/hardware/sim/mcp23017.rs Normal file
View File

@@ -0,0 +1,57 @@
use embedded_hal::i2c::{ErrorKind, ErrorType, I2c, Operation, SevenBitAddress};
use log::trace;
use std::fmt::{Display, Formatter};
#[derive(Debug)]
pub struct SimMcp23017 {}
impl SimMcp23017 {
pub fn new() -> Self {
trace!("SimMcp23017::new()");
Self {}
}
}
#[derive(Debug)]
pub struct ErrorWrapper(anyhow::Error);
impl embedded_hal::i2c::Error for ErrorWrapper {
fn kind(&self) -> ErrorKind {
ErrorKind::Other
}
}
impl Display for ErrorWrapper {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
self.0.fmt(f)
}
}
impl std::error::Error for ErrorWrapper {}
impl ErrorType for SimMcp23017 {
type Error = ErrorWrapper;
}
impl I2c for SimMcp23017 {
fn transaction(
&mut self,
address: SevenBitAddress,
operations: &mut [Operation<'_>],
) -> Result<(), Self::Error> {
trace!(
"SimMcp23017::transaction(self: {self:?}, address: {address}, operations: {operations:?})"
);
for operation in operations {
match operation {
Operation::Write(_write_buffer) => {
// Ignore incoming write operations
}
Operation::Read(_read_buffer) => {
todo!("SimMcp23017 read")
}
}
}
Ok(())
}
}

120
flight/src/hardware/sim/mct8316a.rs Normal file
View File

@@ -0,0 +1,120 @@
use crate::hardware::error::WrappingError;
use anyhow::anyhow;
use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress};
use log::trace;
use std::collections::HashMap;
const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
#[derive(Debug)]
pub struct SimMct8316a {
data: HashMap<u32, u32>,
}
impl SimMct8316a {
pub fn new() -> Self {
trace!("SimMct8316a::new()");
Self {
data: HashMap::new(),
}
}
}
impl ErrorType for SimMct8316a {
type Error = WrappingError<anyhow::Error>;
}
impl I2c for SimMct8316a {
fn transaction(
&mut self,
i2c_addr: SevenBitAddress,
operations: &mut [Operation<'_>],
) -> Result<(), Self::Error> {
trace!(
"SimMct8316a::transaction(self: {self:?}, i2c_addr: {i2c_addr}, operations: {operations:?})"
);
let mut do_read_operation = false;
let mut include_crc = false;
let mut data_length = 2;
let mut address: u32 = 0;
let mut crc = CRC.digest();
for operation in operations {
match operation {
Operation::Write(write_buffer) => {
// Ignore incoming write operations
if write_buffer.len() < 3 {
return Err(WrappingError(anyhow!("Not enough data in write")));
}
// We don't care about the first byte
do_read_operation = write_buffer[0] & 0x80 > 0;
include_crc = write_buffer[0] & 0x40 > 0;
if write_buffer[0] & 0x10 > 0 {
data_length = 4;
} else if write_buffer[0] & 0x20 > 0 {
data_length = 8;
}
address |= u32::from(write_buffer[0] & 0xF) << 16;
address |= u32::from(write_buffer[1]) << 8;
address |= u32::from(write_buffer[2]);
if !do_read_operation {
if write_buffer.len() != 3 + data_length + 1 {
return Err(WrappingError(anyhow!(
"Write for write has wrong length {}. {} expected",
write_buffer.len(),
3 + data_length + 1
)));
}
if data_length == 2 {
todo!("Unimplemented");
} else {
let written_value = u32::from_be_bytes([
write_buffer[3],
write_buffer[4],
write_buffer[5],
write_buffer[6],
]);
self.data.insert(address, written_value);
if data_length == 8 {
todo!("Unimplemented");
}
}
} else if write_buffer.len() != 3 {
return Err(WrappingError(anyhow!(
"Write for read has wrong length {}. {} expected",
write_buffer.len(),
3
)));
}
crc.update(write_buffer);
}
Operation::Read(read_buffer) => {
if !do_read_operation {
return Err(WrappingError(anyhow!("Unexpected Read Operation")));
}
let expected_length = data_length + usize::from(include_crc);
if read_buffer.len() != expected_length {
return Err(WrappingError(anyhow!(
"Unexpected length in read buffer {}. {} expected",
read_buffer.len(),
expected_length
)));
}
crc.update(&[(i2c_addr << 1) | 0b1]);
if data_length == 2 {
todo!("Unimplemented");
} else if data_length == 4 {
let value = *self.data.get(&address).unwrap_or(&0);
read_buffer[0..4].copy_from_slice(&value.to_be_bytes());
} else {
todo!("Unimplemented");
}
crc.update(&read_buffer[0..data_length]);
if do_read_operation {
read_buffer[data_length] = crc.clone().finalize();
}
}
}
}
Ok(())
}
}

12
flight/src/hardware/sim/mod.rs Normal file
View File

@@ -0,0 +1,12 @@
#[cfg(not(feature = "raspi"))]
mod mcp23017;
#[cfg(not(feature = "raspi"))]
mod pwm;
pub(super) mod mct8316a;
#[cfg(not(feature = "raspi"))]
pub mod hardware;
#[cfg(not(feature = "raspi"))]
pub use hardware::SimHardware;

49
flight/src/hardware/sim/pwm.rs Normal file
View File

@@ -0,0 +1,49 @@
use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
use log::trace;
use std::fmt::{Display, Formatter};
#[derive(Debug)]
pub struct SimPwm {
duty_cycle: u16,
}
impl SimPwm {
pub fn new() -> Self {
trace!("SimPwm::new()");
Self { duty_cycle: 0 }
}
}
#[derive(Debug)]
pub struct ErrorWrapper(anyhow::Error);
impl embedded_hal::pwm::Error for ErrorWrapper {
fn kind(&self) -> ErrorKind {
ErrorKind::Other
}
}
impl Display for ErrorWrapper {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
self.0.fmt(f)
}
}
impl std::error::Error for ErrorWrapper {}
impl ErrorType for SimPwm {
type Error = ErrorWrapper;
}
impl SetDutyCycle for SimPwm {
fn max_duty_cycle(&self) -> u16 {
trace!("SimPwm::max_duty_cycle()");
u16::MAX
}
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
trace!("SimPwm::set_duty_cycle(duty: {duty})");
self.duty_cycle = duty;
Ok(())
}
}

117
flight/src/lib.rs
View File

@@ -1,30 +1,33 @@
use crate::hardware::channelization::{MCP23017_A_LED, MCP23017_B_LED}; #![warn(clippy::all, clippy::pedantic)]
use crate::hardware::initialize; use crate::comms::CommsTask;
use crate::hardware::mcp23017::Mcp23017OutputPin;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task};
use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::Hardware; use crate::hardware::Hardware;
use crate::on_drop::on_drop; use crate::hardware::initialize;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::pin::PinDevice;
use crate::scheduler::Scheduler;
use crate::state_vector::StateVector;
use anyhow::Result; use anyhow::Result;
use embedded_hal::digital::PinState;
use embedded_hal::pwm::SetDutyCycle; use embedded_hal::pwm::SetDutyCycle;
use log::info; use log::info;
use std::sync::atomic::{AtomicBool, Ordering}; use nautilus_common::add_ctrlc_handler;
use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
use std::sync::Arc; use std::sync::Arc;
use std::thread; use std::sync::atomic::{AtomicBool, Ordering};
use std::thread::sleep; use std::thread::sleep;
use std::time::Duration; use std::time::Duration;
mod hardware; mod hardware;
fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> Result<()> { fn new_shutdown_handler(running: &Arc<AtomicBool>) -> impl Fn(()) {
ctrlc::set_handler(move || { let running = running.clone();
info!("Shutdown Requested"); move |()| running.store(false, Ordering::Relaxed)
flag.store(false, Ordering::Relaxed);
})?;
Ok(())
} }
/// Run the flight software
///
/// # Errors
/// An unrecoverable error had occurred
pub fn run() -> Result<()> { pub fn run() -> Result<()> {
info!( info!(
"Project Nautilus Flight Software {}", "Project Nautilus Flight Software {}",
@@ -34,6 +37,8 @@ pub fn run() -> Result<()> {
let running = Arc::new(AtomicBool::new(true)); let running = Arc::new(AtomicBool::new(true));
add_ctrlc_handler(running.clone())?; add_ctrlc_handler(running.clone())?;
let state_vector = StateVector::new();
let hal = initialize()?; let hal = initialize()?;
let mut mcp23017_a = hal.new_mcp23017_a()?; let mut mcp23017_a = hal.new_mcp23017_a()?;
@@ -49,40 +54,50 @@ pub fn run() -> Result<()> {
mcp23017_b.init()?; mcp23017_b.init()?;
mct8316.init()?; mct8316.init()?;
thread::scope(|scope| { Scheduler::scope(running.clone(), |s| {
// This will automatically set running to false when it drops let task_a = s.run_cyclic(
// This means that if the main thread exits this scope, we will "mcp23017-a-task",
// shut down any side branches Mcp23017Task::new(mcp23017_a, &state_vector),
let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed)); 10,
)?;
let a_id = task_a.get_id();
Mcp23017Task::new(&mcp23017_a, "mcp23017-a".into()) let task_b = s.run_cyclic(
.start(scope, &running, 10)?; "mcp23017-b-task",
Mcp23017Task::new(mcp23017_b, &state_vector),
10,
)?;
let b_id = task_b.get_id();
Mcp23017Task::new(&mcp23017_b, "mcp23017-b".into()) let mut comms = CommsTask::new(15000, "nautilus-ground:14000")?;
.start(scope, &running, 10)?; 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())?;
let comms = s.run_cyclic("comms-task", comms, 10)?;
let mut led_pin_a = mcp23017_a.new_output_pin(MCP23017_A_LED)?; let sv = &state_vector;
led_pin_a.set_state_on_drop(PinState::Low); s.run_cyclic(
let mut led_pin_b = mcp23017_b.new_output_pin(MCP23017_B_LED)?; "telemetry-producer",
led_pin_b.set_state_on_drop(PinState::Low); move || {
sv.access_section(&a_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::A,
switches: state.pins,
});
});
sv.access_section(&b_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::B,
switches: state.pins,
});
});
},
1,
)?;
info!("Starting Main Loop");
loop { while running.load(Ordering::Relaxed) {
led_pin_a.set_state(PinState::High); sleep(Duration::from_millis(100));
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
led_pin_b.set_state(PinState::High);
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
led_pin_a.set_state(PinState::Low);
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
led_pin_b.set_state(PinState::Low);
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
} }
anyhow::Ok(()) anyhow::Ok(())
@@ -92,17 +107,21 @@ pub fn run() -> Result<()> {
// Explicitly drop these to allow the borrow checker to be sure that // Explicitly drop these to allow the borrow checker to be sure that
// dropping the hal is safe // dropping the hal is safe
drop(mcp23017_a);
drop(mcp23017_b);
drop(pwm0); drop(pwm0);
drop(mct8316); drop(mct8316);
drop(hal); drop(hal);
drop(state_vector);
Ok(()) Ok(())
} }
#[cfg(test)] mod comms;
mod test_utils;
mod data; mod data;
mod on_drop; mod on_drop;
mod rcs;
mod scheduler;
mod state_vector;
#[cfg(test)]
mod test_utils;

12
flight/src/main.rs
View File

@@ -1,15 +1,17 @@
use crate::logger::setup_logger;
use log::error; use log::error;
use nautilus_common::logger::setup_logger;
use nautilus_flight::run; use nautilus_flight::run;
mod logger;
fn main() { fn main() {
setup_logger().expect("Failed to setup logger"); setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger");
match run() { match run() {
Ok(_) => {} Ok(_) => {}
Err(err) => { Err(err) => {
error!("An unhandled error occurred: {}\n\n{}", err, err.backtrace()); error!(
"An unhandled error occurred: {}\n\n{}",
err,
err.backtrace()
);
} }
} }
} }

10
flight/src/on_drop.rs
View File

@@ -1,18 +1,18 @@
use log::trace; use log::trace;
use std::any::type_name;
pub struct OnDrop<F: FnMut()> { pub struct OnDrop<F: FnMut()> {
func: F, func: F,
} }
pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> { pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> {
OnDrop { trace!("on_drop<F={}>()", type_name::<F>());
func OnDrop { func }
}
} }
impl<F: FnMut()> Drop for OnDrop<F> { impl<F: FnMut()> Drop for OnDrop<F> {
fn drop(&mut self) { fn drop(&mut self) {
trace!("OnDrop::drop()"); trace!("OnDrop<F={}>::drop()", type_name::<F>());
(self.func)() (self.func)();
} }
} }

13
flight/src/rcs/mod.rs Normal file
View File

@@ -0,0 +1,13 @@
// use crate::hardware::mcp23017::Mcp23017OutputPin;
//
// struct RcsTask<PIN: Mcp23017OutputPin> {
// pin: PIN,
// }
//
// impl<PIN: Mcp23017OutputPin> RcsTask<PIN> {
// pub fn new(pin: PIN) -> Self {
// Self {
// pin
// }
// }
// }

149
flight/src/scheduler/mod.rs Normal file
View File

@@ -0,0 +1,149 @@
use crate::on_drop::on_drop;
use anyhow::Result;
use log::trace;
use std::any::type_name;
use std::fmt::Debug;
use std::ops::Deref;
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::{Receiver, Sender, channel};
use std::thread;
use std::thread::{Scope, sleep};
use std::time::{Duration, Instant};
#[derive(Clone, Debug)]
pub struct TaskHandle<Message, Data> {
#[allow(dead_code)]
pub name: String,
pub sender: Sender<Message>,
data: Data,
}
impl<Message, Data> Deref for TaskHandle<Message, Data> {
type Target = Data;
fn deref(&self) -> &Self::Target {
&self.data
}
}
#[allow(dead_code)]
pub trait Task {
type Message;
type Data;
fn get_data(&self) -> Self::Data;
fn run(self, receiver: Receiver<Self::Message>, running: Arc<AtomicBool>);
}
pub trait CyclicTask {
type Message;
type Data;
fn get_data(&self) -> Self::Data;
fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant);
}
impl<F> CyclicTask for F
where
F: Fn(),
{
type Message = ();
type Data = ();
fn get_data(&self) -> Self::Data {}
fn step(&mut self, _receiver: &Receiver<Self::Message>, _step_time: Instant) {
self();
}
}
pub struct Scheduler<'s, 'e> {
scope: &'s Scope<'s, 'e>,
running: Arc<AtomicBool>,
}
impl<'s> Scheduler<'s, '_> {
pub fn scope<'env, F, R>(running: Arc<AtomicBool>, f: F) -> R
where
F: FnOnce(Scheduler<'_, 'env>) -> R,
{
trace!("Scheduler::new(running: {running:?}, f)");
thread::scope(|scope: &Scope| {
let running_result = running.clone();
// 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(move || running.store(false, Ordering::Relaxed));
f(Scheduler {
scope,
running: running_result,
})
})
}
#[allow(dead_code)]
pub fn run<'t, T>(
&self,
name: impl Into<String>,
task: T,
) -> Result<TaskHandle<T::Message, T::Data>>
where
T: Task + Send + Debug + 't,
T::Message: Send,
't: 's,
{
let name = name.into();
trace!(
"Scheduler::run<T={}>(name: {name}, task: {task:?})",
type_name::<T>()
);
let running = self.running.clone();
let (sender, receiver) = channel::<T::Message>();
let data = task.get_data();
let _ = thread::Builder::new()
.name(name.clone())
.spawn_scoped(self.scope, move || {
task.run(receiver, running);
})?;
Ok(TaskHandle { name, sender, data })
}
pub fn run_cyclic<'t, T>(
&self,
name: impl Into<String>,
mut task: T,
frequency: u64,
) -> Result<TaskHandle<T::Message, T::Data>>
where
T: CyclicTask + Send + 't,
T::Message: Send,
't: 's,
{
let name = name.into();
trace!(
"Scheduler::run_cyclic<T={}>(name: {name}, task, frequency: {frequency})",
type_name::<T>()
);
let running = self.running.clone();
let (sender, receiver) = channel::<T::Message>();
let data = task.get_data();
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, data })
}
}

165
flight/src/state_vector/mod.rs Normal file
View File

@@ -0,0 +1,165 @@
use log::trace;
use std::any::{Any, type_name};
use std::collections::HashMap;
use std::fmt::{Debug, Formatter};
use std::marker::PhantomData;
use std::sync::RwLock;
use std::sync::atomic::{AtomicUsize, Ordering};
#[derive(Debug)]
pub struct StateVector {
next_section: AtomicUsize,
sections: RwLock<HashMap<SectionIdentifier, Box<RwLock<dyn Any + Send + Sync>>>>,
}
#[derive(Clone, Eq, PartialEq, Hash, Debug)]
pub struct SectionIdentifier(usize);
pub struct SectionWriter<'a, T> {
id: SectionIdentifier,
state_vector: &'a StateVector,
_phantom_data: PhantomData<T>,
}
impl<T> Debug for SectionWriter<'_, T> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(
f,
"SectionWriter<T={}> {{ id: {:?}, state_vector: {:?} }}",
type_name::<T>(),
self.id,
self.state_vector
)
}
}
impl<T: 'static> SectionWriter<'_, T> {
pub fn get_identifier(&self) -> SectionIdentifier {
trace!(
"SectionWriter<T={}>::get_identifier(self: {self:?})",
type_name::<T>()
);
self.id.clone()
}
pub fn update<F, R>(&self, f: F) -> R
where
F: FnOnce(&mut T) -> R,
{
trace!(
"SectionWriter<T={}>::update(self: {self:?}, f)",
type_name::<T>()
);
self.state_vector.sections.clear_poison();
let sections = self.state_vector.sections.read().unwrap();
let section = sections.get(&self.id).unwrap();
let mut data = section.write().unwrap();
let result = data.downcast_mut::<T>().unwrap();
f(result)
}
}
impl StateVector {
pub fn new() -> Self {
trace!("StateVector::new()");
Self {
next_section: AtomicUsize::new(0usize),
sections: RwLock::new(HashMap::new()),
}
}
pub fn create_section<T>(&self, initial_value: T) -> SectionWriter<'_, T>
where
T: Send + Sync + 'static,
{
trace!(
"StateVector::create_section<T={}>(self: {self:?}, initial_value)",
type_name::<T>()
);
let id = SectionIdentifier(self.next_section.fetch_add(1usize, Ordering::SeqCst));
let lock = Box::new(RwLock::new(initial_value));
self.sections.clear_poison();
let mut sections = self.sections.write().unwrap();
if !sections.contains_key(&id) {
sections.insert(id.clone(), lock);
}
drop(sections);
SectionWriter {
id,
state_vector: self,
_phantom_data: PhantomData,
}
}
pub fn access_section<T, F, R>(&self, id: &SectionIdentifier, f: F) -> Option<R>
where
T: 'static,
F: FnOnce(&T) -> R,
{
trace!(
"StateVector::access_section<T={}, F={}, R={}>(self: {self:?}, id: {id:?}, f)",
type_name::<T>(),
type_name::<F>(),
type_name::<R>()
);
self.sections.clear_poison();
let Ok(sections) = self.sections.read() else {
return None;
};
let section = sections.get(id)?;
section.clear_poison();
let Ok(data) = section.read() else {
return None;
};
let inner = data.downcast_ref::<T>()?;
Some(f(inner))
}
}
#[cfg(test)]
mod tests {
use super::*;
use anyhow::Result;
#[derive(Default)]
struct TestType {
value1: i32,
value2: i32,
}
#[test]
fn test_two_sections() -> Result<()> {
let state_vector = StateVector::new();
let section_1 = state_vector.create_section(TestType::default());
let section_2 = state_vector.create_section(TestType::default());
section_1.update(|s| {
s.value1 = 1;
s.value2 = 2;
});
section_2.update(|s| {
s.value1 = 3;
s.value2 = 4;
});
let id_1 = section_1.get_identifier();
state_vector.access_section(&id_1, |s: &TestType| {
assert_eq!(1, s.value1);
assert_eq!(2, s.value2);
});
let id_2 = section_2.get_identifier();
state_vector.access_section(&id_2, |s: &TestType| {
assert_eq!(3, s.value1);
assert_eq!(4, s.value2);
});
Ok(())
}
}

9
flight/src/test_utils.rs
View File

@@ -1,3 +1,4 @@
#![allow(dead_code)]
use std::mem::ManuallyDrop; use std::mem::ManuallyDrop;
use std::ops::{Deref, DerefMut}; use std::ops::{Deref, DerefMut};
use std::panic; use std::panic;
@@ -9,7 +10,7 @@ pub struct SilentDrop<T: panic::UnwindSafe> {
impl<T: panic::UnwindSafe> SilentDrop<T> { impl<T: panic::UnwindSafe> SilentDrop<T> {
pub fn new(inner: T) -> Self { pub fn new(inner: T) -> Self {
Self { Self {
inner: ManuallyDrop::new(inner) inner: ManuallyDrop::new(inner),
} }
} }
} }
@@ -32,13 +33,11 @@ impl<T: panic::UnwindSafe> Drop for SilentDrop<T> {
fn drop(&mut self) { fn drop(&mut self) {
let prev_hook = panic::take_hook(); let prev_hook = panic::take_hook();
panic::set_hook(Box::new(|_| {})); panic::set_hook(Box::new(|_| {}));
let inner = unsafe { let inner = unsafe { ManuallyDrop::take(&mut self.inner) };
ManuallyDrop::take(&mut self.inner)
};
let destroy_closure = || { let destroy_closure = || {
drop(inner); drop(inner);
}; };
if panic::catch_unwind(destroy_closure).is_err() {}; if panic::catch_unwind(destroy_closure).is_err() {};
panic::set_hook(prev_hook); panic::set_hook(prev_hook);
} }
} }

7
ground/Cargo.toml
View File

@@ -4,3 +4,10 @@ version = "0.1.0"
edition = "2024" edition = "2024"
[dependencies] [dependencies]
anyhow = { workspace = true }
nautilus_common = { path = "../common" }
log = { workspace = true }
chrono = { workspace = true }
postcard = { workspace = true }
hex = "0.4.3"
serde = { workspace = true }

106
ground/src/lib.rs Normal file
View File

@@ -0,0 +1,106 @@
#![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 std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::time::Duration;
/// Run the Ground Software
///
/// # Errors
/// If any unhandled error occurred in the Ground Software
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 = [0u8; 512];
let mut do_once = true;
while running.load(Ordering::Relaxed) {
match udp.recv_postcard::<Telemetry>(&mut buffer) {
Ok((tlm, addr)) => {
flight_addr = Some(addr);
info!("{tlm:?}");
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}");
}
}
}
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])?;
}
Ok(())
}

16
ground/src/main.rs
View File

@@ -1,3 +1,17 @@
use log::error;
use nautilus_common::logger::setup_logger;
use nautilus_ground::run;
fn main() { 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()
);
}
}
} }