Compare commits
10 Commits
086ac7f195
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 59b5679dda | |||
| ea56b9865e | |||
| d53d78434c | |||
| eefc3293b4 | |||
| 5455935f3a | |||
| e0f17649b2 | |||
| b067ae5cec | |||
| fd63bdc0c9 | |||
| 26271fcb17 | |||
| 2bcb122319 |
322
Cargo.lock
generated
322
Cargo.lock
generated
@@ -17,15 +17,6 @@ version = "1.0.100"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a23eb6b1614318a8071c9b2521f36b424b2c83db5eb3a0fead4a6c0809af6e61"
|
||||
|
||||
[[package]]
|
||||
name = "approx"
|
||||
version = "0.5.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "autocfg"
|
||||
version = "1.4.0"
|
||||
@@ -44,12 +35,6 @@ version = "3.17.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf"
|
||||
|
||||
[[package]]
|
||||
name = "bytemuck"
|
||||
version = "1.23.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9134a6ef01ce4b366b50689c94f82c14bc72bc5d0386829828a2e2752ef7958c"
|
||||
|
||||
[[package]]
|
||||
name = "cc"
|
||||
version = "1.2.17"
|
||||
@@ -80,10 +65,20 @@ dependencies = [
|
||||
"iana-time-zone",
|
||||
"js-sys",
|
||||
"num-traits",
|
||||
"serde",
|
||||
"wasm-bindgen",
|
||||
"windows-link",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "cobs"
|
||||
version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0fa961b519f0b462e3a3b4a34b64d119eeaca1d59af726fe450bbba07a9fc0a1"
|
||||
dependencies = [
|
||||
"thiserror",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "colored"
|
||||
version = "2.2.0"
|
||||
@@ -188,6 +183,18 @@ dependencies = [
|
||||
"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]]
|
||||
name = "embedded-time"
|
||||
version = "0.12.1"
|
||||
@@ -207,102 +214,6 @@ dependencies = [
|
||||
"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]]
|
||||
name = "hex"
|
||||
version = "0.4.3"
|
||||
@@ -361,62 +272,19 @@ version = "0.4.28"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432"
|
||||
|
||||
[[package]]
|
||||
name = "matrixmultiply"
|
||||
version = "0.3.10"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
"rawpointer",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nalgebra"
|
||||
version = "0.34.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c4d5b3eff5cd580f93da45e64715e8c20a3996342f1e466599cf7a267a0c2f5f"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"glam 0.14.0",
|
||||
"glam 0.15.2",
|
||||
"glam 0.16.0",
|
||||
"glam 0.17.3",
|
||||
"glam 0.18.0",
|
||||
"glam 0.19.0",
|
||||
"glam 0.20.5",
|
||||
"glam 0.21.3",
|
||||
"glam 0.22.0",
|
||||
"glam 0.23.0",
|
||||
"glam 0.24.2",
|
||||
"glam 0.25.0",
|
||||
"glam 0.27.0",
|
||||
"glam 0.28.0",
|
||||
"glam 0.29.3",
|
||||
"glam 0.30.8",
|
||||
"matrixmultiply",
|
||||
"nalgebra-macros",
|
||||
"num-complex 0.4.6",
|
||||
"num-rational 0.4.2",
|
||||
"num-traits",
|
||||
"simba",
|
||||
"typenum",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nalgebra-macros"
|
||||
version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nautilus_common"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"anyhow",
|
||||
"chrono",
|
||||
"ctrlc",
|
||||
"fern",
|
||||
"log",
|
||||
"postcard",
|
||||
"serde",
|
||||
"thiserror",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nautilus_flight"
|
||||
@@ -425,22 +293,28 @@ dependencies = [
|
||||
"anyhow",
|
||||
"chrono",
|
||||
"crc",
|
||||
"ctrlc",
|
||||
"embedded-hal 1.0.0",
|
||||
"embedded-hal-bus",
|
||||
"embedded-hal-mock",
|
||||
"fern",
|
||||
"hex",
|
||||
"log",
|
||||
"nalgebra",
|
||||
"num-traits",
|
||||
"nautilus_common",
|
||||
"postcard",
|
||||
"rpi-pal",
|
||||
"thiserror",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nautilus_ground"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"anyhow",
|
||||
"chrono",
|
||||
"hex",
|
||||
"log",
|
||||
"nautilus_common",
|
||||
"postcard",
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nb"
|
||||
@@ -475,20 +349,10 @@ version = "0.3.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
|
||||
dependencies = [
|
||||
"num-complex 0.3.1",
|
||||
"num-complex",
|
||||
"num-integer",
|
||||
"num-iter",
|
||||
"num-rational 0.3.2",
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-bigint"
|
||||
version = "0.4.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9"
|
||||
dependencies = [
|
||||
"num-integer",
|
||||
"num-rational",
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
@@ -501,15 +365,6 @@ dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-complex"
|
||||
version = "0.4.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-integer"
|
||||
version = "0.1.46"
|
||||
@@ -541,17 +396,6 @@ dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-rational"
|
||||
version = "0.4.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
|
||||
dependencies = [
|
||||
"num-bigint",
|
||||
"num-integer",
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-traits"
|
||||
version = "0.2.19"
|
||||
@@ -568,10 +412,16 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d"
|
||||
|
||||
[[package]]
|
||||
name = "paste"
|
||||
version = "1.0.15"
|
||||
name = "postcard"
|
||||
version = "1.1.3"
|
||||
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]]
|
||||
name = "proc-macro2"
|
||||
@@ -591,12 +441,6 @@ dependencies = [
|
||||
"proc-macro2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rawpointer"
|
||||
version = "0.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||
|
||||
[[package]]
|
||||
name = "rpi-pal"
|
||||
version = "0.22.2"
|
||||
@@ -619,12 +463,33 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2"
|
||||
|
||||
[[package]]
|
||||
name = "safe_arch"
|
||||
version = "0.7.4"
|
||||
name = "serde"
|
||||
version = "1.0.228"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323"
|
||||
checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e"
|
||||
dependencies = [
|
||||
"bytemuck",
|
||||
"serde_core",
|
||||
"serde_derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_core"
|
||||
version = "1.0.228"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad"
|
||||
dependencies = [
|
||||
"serde_derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_derive"
|
||||
version = "1.0.228"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@@ -633,19 +498,6 @@ version = "1.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
|
||||
|
||||
[[package]]
|
||||
name = "simba"
|
||||
version = "0.9.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b3a386a501cd104797982c15ae17aafe8b9261315b5d07e3ec803f2ea26be0fa"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"num-complex 0.4.6",
|
||||
"num-traits",
|
||||
"paste",
|
||||
"wide",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "spin_sleep"
|
||||
version = "1.3.1"
|
||||
@@ -686,12 +538,6 @@ dependencies = [
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "typenum"
|
||||
version = "1.18.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f"
|
||||
|
||||
[[package]]
|
||||
name = "unicode-ident"
|
||||
version = "1.0.18"
|
||||
@@ -762,16 +608,6 @@ dependencies = [
|
||||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "wide"
|
||||
version = "0.7.32"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22"
|
||||
dependencies = [
|
||||
"bytemuck",
|
||||
"safe_arch",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "windows-core"
|
||||
version = "0.52.0"
|
||||
|
||||
12
Cargo.toml
12
Cargo.toml
@@ -1,3 +1,13 @@
|
||||
[workspace]
|
||||
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"
|
||||
|
||||
@@ -4,3 +4,11 @@ version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[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
78
common/src/command/mod.rs
Normal 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
|
||||
}
|
||||
}
|
||||
@@ -1,14 +1,21 @@
|
||||
pub fn add(left: u64, right: u64) -> u64 {
|
||||
left + right
|
||||
}
|
||||
#![warn(clippy::all, clippy::pedantic)]
|
||||
use log::info;
|
||||
use std::sync::Arc;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
pub mod command;
|
||||
pub mod logger;
|
||||
pub mod telemetry;
|
||||
pub mod udp;
|
||||
|
||||
#[test]
|
||||
fn it_works() {
|
||||
let result = add(2, 2);
|
||||
assert_eq!(result, 4);
|
||||
}
|
||||
/// Add a ctrl-c handler which will set an atomic flag to `false` when ctrl-c is detected
|
||||
///
|
||||
/// # Errors
|
||||
/// If a system error occurred while trying to set the ctrl-c handler
|
||||
pub fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> anyhow::Result<()> {
|
||||
ctrlc::set_handler(move || {
|
||||
info!("Shutdown Requested");
|
||||
flag.store(false, Ordering::Relaxed);
|
||||
})?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
@@ -5,13 +5,17 @@ use std::fs::create_dir_all;
|
||||
use std::str::FromStr;
|
||||
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(|_| {
|
||||
create_dir_all("logs/")?;
|
||||
|
||||
anyhow::Ok(format!(
|
||||
"logs/{}_{}.log",
|
||||
env!("CARGO_PKG_NAME"),
|
||||
package_name,
|
||||
chrono::Local::now().format("%Y%m%d_%H%M%S")
|
||||
))
|
||||
})?;
|
||||
@@ -37,7 +41,7 @@ pub fn setup_logger() -> Result<()> {
|
||||
level = colors.color(record.level()),
|
||||
time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
|
||||
target = record.target(),
|
||||
))
|
||||
));
|
||||
})
|
||||
.chain(
|
||||
fern::Dispatch::new()
|
||||
@@ -47,6 +51,6 @@ pub fn setup_logger() -> Result<()> {
|
||||
.chain(fern::log_file(log_file.clone())?)
|
||||
.apply()?;
|
||||
|
||||
debug!("Logging to {} at level {}", log_file, log_level);
|
||||
debug!("Logging to {log_file} at level {log_level}");
|
||||
Ok(())
|
||||
}
|
||||
24
common/src/telemetry/mod.rs
Normal file
24
common/src/telemetry/mod.rs
Normal 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
140
common/src/udp.rs
Normal 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(())
|
||||
}
|
||||
}
|
||||
@@ -4,20 +4,17 @@ version = "0.0.1"
|
||||
edition = "2024"
|
||||
|
||||
[dependencies]
|
||||
anyhow = "1.0.100"
|
||||
fern = { version = "0.7.1", features = ["colored"] }
|
||||
log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] }
|
||||
chrono = "0.4.42"
|
||||
anyhow = { workspace = true }
|
||||
log = { workspace = true, features = ["max_level_trace", "release_max_level_debug"] }
|
||||
chrono = { workspace = true }
|
||||
embedded-hal = "1.0.0"
|
||||
embedded-hal-bus = { version = "0.3.0", features = ["std"] }
|
||||
embedded-hal-mock = { version = "0.11.1", optional = true }
|
||||
rpi-pal = { version = "0.22.2", features = ["hal"], optional = true }
|
||||
nalgebra = "0.34.1"
|
||||
hex = "0.4.3"
|
||||
thiserror = "2.0.17"
|
||||
num-traits = "0.2.19"
|
||||
crc = "3.3.0"
|
||||
ctrlc = { version = "3.5" }
|
||||
nautilus_common = { path = "../common" }
|
||||
postcard = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
embedded-hal-mock = { version = "0.11.1" }
|
||||
|
||||
146
flight/src/comms/mod.rs
Normal file
146
flight/src/comms/mod.rs
Normal 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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,4 @@
|
||||
#[cfg(not(feature = "raspi"))]
|
||||
pub mod reader;
|
||||
#[cfg(not(feature = "raspi"))]
|
||||
pub mod writer;
|
||||
// #[cfg(not(feature = "raspi"))]
|
||||
// pub mod reader;
|
||||
// #[cfg(not(feature = "raspi"))]
|
||||
// pub mod writer;
|
||||
|
||||
@@ -1,2 +1,95 @@
|
||||
pub const MCP23017_A_LED: u8 = 7;
|
||||
pub const MCP23017_B_LED: u8 = 7;
|
||||
#![allow(dead_code)]
|
||||
|
||||
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 }),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
3
flight/src/hardware/error/mod.rs
Normal file
@@ -0,0 +1,3 @@
|
||||
mod wrapping;
|
||||
|
||||
pub use wrapping::WrappingError;
|
||||
26
flight/src/hardware/error/wrapping.rs
Normal file
26
flight/src/hardware/error/wrapping.rs
Normal 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
|
||||
}
|
||||
}
|
||||
@@ -1,11 +1,11 @@
|
||||
use crate::hardware::error::I2cError;
|
||||
use crate::hardware::mcp23017::pin::{Mcp23017OutputPinDriver, Mcp23017Pins};
|
||||
use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin};
|
||||
use crate::hardware::error::WrappingError;
|
||||
use crate::hardware::mcp23017::Mcp23017;
|
||||
use anyhow::bail;
|
||||
use embedded_hal::digital::PinState;
|
||||
use embedded_hal::i2c::I2c;
|
||||
use log::{error, trace};
|
||||
use std::any::type_name;
|
||||
use std::fmt::{Debug, Formatter};
|
||||
use std::sync::atomic::{AtomicU16, Ordering};
|
||||
use std::sync::Mutex;
|
||||
|
||||
pub struct Mcp23017Driver<I2C>
|
||||
@@ -17,9 +17,8 @@ where
|
||||
{
|
||||
i2c: Mutex<I2C>,
|
||||
address: u8,
|
||||
bank: AtomicU16,
|
||||
last_flushed: AtomicU16,
|
||||
in_use: AtomicU16,
|
||||
bank: u16,
|
||||
last_flushed: u16,
|
||||
}
|
||||
|
||||
impl<I2C> Debug for Mcp23017Driver<I2C>
|
||||
@@ -30,7 +29,12 @@ where
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
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,
|
||||
{
|
||||
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 {
|
||||
i2c: i2c.into(),
|
||||
address,
|
||||
bank: AtomicU16::new(0),
|
||||
last_flushed: AtomicU16::new(0),
|
||||
in_use: AtomicU16::new(0),
|
||||
bank: 0,
|
||||
last_flushed: 0,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -62,40 +68,13 @@ where
|
||||
{
|
||||
fn drop(&mut 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() {
|
||||
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>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
@@ -109,49 +88,38 @@ where
|
||||
let data: [u8; _] = [0x00, 0x00, 0x00];
|
||||
|
||||
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"),
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn new_output_pin(&self, pin: u8) -> anyhow::Result<impl Mcp23017OutputPin> {
|
||||
trace!("Mcp23017Driver::new_output_pin(self: {self:?}, pin: {pin})");
|
||||
fn set_pin(&mut self, pin: u8, value: PinState) -> anyhow::Result<()> {
|
||||
trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
|
||||
if !(0u8..16u8).contains(&pin) {
|
||||
bail!("Invalid Pin ID")
|
||||
}
|
||||
|
||||
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 {
|
||||
mcp23017: self,
|
||||
pin,
|
||||
on_drop: None,
|
||||
})
|
||||
self.bank = match value {
|
||||
PinState::Low => self.bank & !pin_mask,
|
||||
PinState::High => self.bank | pin_mask,
|
||||
};
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn flush(&self) -> anyhow::Result<()> {
|
||||
fn flush(&mut self) -> anyhow::Result<()> {
|
||||
trace!("Mcp23017Driver::flush(self: {self:?})");
|
||||
// Relaxed because we don't care about the value
|
||||
let bank = self.bank.load(Ordering::Relaxed);
|
||||
// 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();
|
||||
if self.bank != self.last_flushed {
|
||||
let bytes = self.bank.to_le_bytes();
|
||||
let data: [u8; _] = [0x12, bytes[0], bytes[1]];
|
||||
// This blocks while writing
|
||||
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.last_flushed = self.bank;
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
|
||||
@@ -1,4 +1,3 @@
|
||||
mod pin;
|
||||
mod driver;
|
||||
mod task;
|
||||
|
||||
@@ -8,16 +7,11 @@ use embedded_hal::digital::PinState;
|
||||
pub trait Mcp23017 {
|
||||
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<()>;
|
||||
}
|
||||
|
||||
pub trait Mcp23017OutputPin {
|
||||
fn set_state(&mut self, pin_state: PinState);
|
||||
|
||||
fn set_state_on_drop(&mut self, pin_state: PinState);
|
||||
fn flush(&mut self) -> Result<()>;
|
||||
}
|
||||
|
||||
pub use driver::Mcp23017Driver;
|
||||
pub use task::Mcp23017State;
|
||||
pub use task::Mcp23017Task;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -1,36 +1,235 @@
|
||||
use crate::hardware::mcp23017::Mcp23017;
|
||||
use anyhow::Result;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::thread;
|
||||
use std::thread::{sleep, Scope, ScopedJoinHandle};
|
||||
use std::time::Duration;
|
||||
use crate::hardware::pin::PinDevice;
|
||||
use crate::scheduler::{CyclicTask, TaskHandle};
|
||||
use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
|
||||
use embedded_hal::digital::PinState;
|
||||
use log::trace;
|
||||
use std::fmt::{Debug, Formatter};
|
||||
use std::sync::mpsc::Receiver;
|
||||
use std::time::Instant;
|
||||
|
||||
pub struct Mcp23017Task<'a, M: Mcp23017 + Sync> {
|
||||
mcp23017: &'a M,
|
||||
name: String,
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum Mcp23017Message {
|
||||
SetPin {
|
||||
pin: u8,
|
||||
value: PinState,
|
||||
valid_until: Instant,
|
||||
priority: u8,
|
||||
},
|
||||
}
|
||||
|
||||
impl<'a, M: Mcp23017 + Sync> Mcp23017Task<'a, M> {
|
||||
pub fn new(
|
||||
mcp23017: &'a M,
|
||||
name: String,
|
||||
) -> Self {
|
||||
impl<D: Debug> PinDevice for TaskHandle<Mcp23017Message, D> {
|
||||
fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) {
|
||||
trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
|
||||
// This can only fail if the other end is disconnected - which we intentionally want to
|
||||
// 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 {
|
||||
mcp23017,
|
||||
name,
|
||||
pins: [PinData::new(); _],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[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, ()>> {
|
||||
let period = Duration::from_nanos(1_000_000_000 / frequency);
|
||||
fn evaluate(&mut self, now: Instant) {
|
||||
trace!("PinData::evaluate(self: {self:?}, now: {now:?})");
|
||||
// Do this twice to check both the current and the current next
|
||||
// If the current is currently invalid, we'd upgrade the next to current
|
||||
for _ in 0..2 {
|
||||
let is_current_valid = self.valid_until.is_some_and(|current| current >= now);
|
||||
if is_current_valid {
|
||||
self.value = self.state;
|
||||
return;
|
||||
}
|
||||
|
||||
Ok(thread::Builder::new()
|
||||
.name(self.name)
|
||||
.spawn_scoped(scope, move || {
|
||||
while running.load(Ordering::Relaxed) {
|
||||
let _ = self.mcp23017.flush();
|
||||
sleep(period);
|
||||
if self.valid_until.is_some() {
|
||||
self.changed = true;
|
||||
}
|
||||
self.state = self.next_state;
|
||||
self.valid_until = self.next_validity;
|
||||
self.priority = self.next_priority;
|
||||
|
||||
self.next_validity = None;
|
||||
self.next_priority = 0;
|
||||
}
|
||||
|
||||
self.value = self.default;
|
||||
}
|
||||
|
||||
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
|
||||
trace!(
|
||||
"PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
|
||||
);
|
||||
let can_replace_current = self
|
||||
.valid_until
|
||||
.is_none_or(|current| current <= valid_until)
|
||||
|| self.priority == priority;
|
||||
let can_replace_next = self.next_validity.is_none_or(|next| next <= valid_until)
|
||||
|| self.next_priority == priority;
|
||||
|
||||
if priority >= self.priority {
|
||||
// This is now the highest priority thing (or most recent of equal priority)
|
||||
if can_replace_current {
|
||||
if can_replace_next {
|
||||
self.next_validity = None;
|
||||
self.next_priority = 0;
|
||||
}
|
||||
})?)
|
||||
} else {
|
||||
self.next_state = self.state;
|
||||
self.next_validity = self.valid_until;
|
||||
self.next_priority = self.priority;
|
||||
}
|
||||
self.state = value;
|
||||
self.valid_until = Some(valid_until);
|
||||
self.priority = priority;
|
||||
self.changed = true;
|
||||
self.value = value;
|
||||
} else {
|
||||
// This is not the highest priority thing
|
||||
if self.priority >= self.next_priority {
|
||||
// Higher priority than the next highest though
|
||||
self.next_state = value;
|
||||
self.next_validity = Some(valid_until);
|
||||
self.next_priority = priority;
|
||||
self.changed = true;
|
||||
} else {
|
||||
// Not high enough priority to remember
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,7 +1,8 @@
|
||||
use crate::hardware::error::SpiError;
|
||||
use anyhow::{ensure, Result};
|
||||
use crate::hardware::error::WrappingError;
|
||||
use anyhow::{Result, ensure};
|
||||
use embedded_hal::spi::SpiDevice;
|
||||
use log::trace;
|
||||
use std::any::type_name;
|
||||
use std::fmt::{Debug, Formatter};
|
||||
|
||||
pub struct Mcp3208<SPI> {
|
||||
@@ -11,7 +12,12 @@ pub struct Mcp3208<SPI> {
|
||||
|
||||
impl<SPI> Debug for Mcp3208<SPI> {
|
||||
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,
|
||||
{
|
||||
pub fn new(spi: SPI, vref: f64) -> Self {
|
||||
trace!("Mcp3208::new(spi, vref: {vref})");
|
||||
Self {
|
||||
spi,
|
||||
vref,
|
||||
}
|
||||
trace!(
|
||||
"Mcp3208<SPI={}>::new(spi, vref: {vref})",
|
||||
type_name::<SPI>()
|
||||
);
|
||||
Self { spi, vref }
|
||||
}
|
||||
|
||||
pub fn read_single(&mut self, channel: u8) -> Result<f64> {
|
||||
@@ -40,7 +46,9 @@ where
|
||||
write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF;
|
||||
|
||||
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;
|
||||
|
||||
|
||||
@@ -249,7 +249,6 @@ pub enum DutyCycleThreshold {
|
||||
Percent2_5 = 0x7,
|
||||
}
|
||||
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct ClosedLoop3 {
|
||||
pub degauss_samples: DegaussSamples,
|
||||
@@ -481,5 +480,3 @@ pub enum FastBrakeDelta {
|
||||
Percent4 = 0x6,
|
||||
Percent5 = 0x7,
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -35,4 +35,3 @@ pub enum PowerMode {
|
||||
ClosedLoop = 0x1,
|
||||
PowerLimit = 0x2,
|
||||
}
|
||||
|
||||
|
||||
@@ -1,20 +1,60 @@
|
||||
use crate::hardware::error::I2cError;
|
||||
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::error::WrappingError;
|
||||
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_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::fault_config::{AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit, FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime, LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage, MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold};
|
||||
use crate::hardware::mct8316a::gate_driver_config::{BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier, 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 crate::hardware::mct8316a::Mct8316a;
|
||||
use anyhow::{bail, ensure, Result};
|
||||
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::gate_driver_config::{
|
||||
BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier,
|
||||
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 log::trace;
|
||||
use std::any::type_name;
|
||||
use std::fmt::{Debug, Display, Formatter};
|
||||
use std::sync::Mutex;
|
||||
|
||||
@@ -38,6 +78,7 @@ impl Display for Mct8316AVData {
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
enum OperationRW {
|
||||
Read,
|
||||
Write,
|
||||
@@ -49,16 +90,16 @@ fn control_word(
|
||||
data: Mct8316AVData,
|
||||
address: u32,
|
||||
) -> [u8; 3] {
|
||||
trace!(
|
||||
"control_word(operation_rw: {operation_rw:?}, crc: {crc}, data: {data}, address: {address:06x})"
|
||||
);
|
||||
let mut control_word = [0u8; _];
|
||||
|
||||
control_word[0] |= match operation_rw {
|
||||
OperationRW::Read => 0x80,
|
||||
OperationRW::Write => 0x00,
|
||||
};
|
||||
control_word[0] |= match crc {
|
||||
true => 0x40,
|
||||
false => 0x00,
|
||||
};
|
||||
control_word[0] |= if crc { 0x40 } else { 0x00 };
|
||||
control_word[0] |= match data {
|
||||
Mct8316AVData::Two(_) => 0x00,
|
||||
Mct8316AVData::Four(_) => 0x10,
|
||||
@@ -66,7 +107,7 @@ fn control_word(
|
||||
};
|
||||
control_word[0] |= ((address >> 16) & 0x0F) 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
|
||||
}
|
||||
@@ -90,7 +131,12 @@ where
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
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,
|
||||
{
|
||||
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 {
|
||||
i2c: i2c.into(),
|
||||
address,
|
||||
}
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
|
||||
trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");
|
||||
|
||||
@@ -118,12 +168,7 @@ where
|
||||
// 1 for crc
|
||||
let mut write_data = [0u8; 1 + 3 + 8 + 1];
|
||||
write_data[0] = (self.address << 1) | 0b0;
|
||||
write_data[1..4].copy_from_slice(&control_word(
|
||||
OperationRW::Write,
|
||||
true,
|
||||
data,
|
||||
address,
|
||||
));
|
||||
write_data[1..4].copy_from_slice(&control_word(OperationRW::Write, true, data, address));
|
||||
let data_length = match data {
|
||||
Mct8316AVData::Two(val) => {
|
||||
write_data[4..6].copy_from_slice(&val.to_be_bytes());
|
||||
@@ -142,13 +187,16 @@ where
|
||||
write_data[4 + data_length] = crc_result;
|
||||
|
||||
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"),
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
|
||||
trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
|
||||
// 1 target id
|
||||
@@ -157,13 +205,9 @@ where
|
||||
// 8 data bytes
|
||||
// 1 crc
|
||||
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[1..4].copy_from_slice(&control_word(
|
||||
OperationRW::Read,
|
||||
true,
|
||||
*data,
|
||||
address,
|
||||
));
|
||||
read_data[1..4].copy_from_slice(&control_word(OperationRW::Read, true, *data, address));
|
||||
read_data[4] = (self.address << 1) | 0b1;
|
||||
let data_length = match data {
|
||||
Mct8316AVData::Two(_) => 2,
|
||||
@@ -175,32 +219,42 @@ where
|
||||
|
||||
let mut i2c_ops = [
|
||||
Operation::Write(&left[1..4]),
|
||||
Operation::Read(&mut right[..(data_length + 1)])
|
||||
Operation::Read(&mut right[..=data_length]),
|
||||
];
|
||||
|
||||
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"),
|
||||
}
|
||||
drop(i2c_ops);
|
||||
|
||||
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 {
|
||||
Mct8316AVData::Two(val) => {
|
||||
*val = u16::from_be_bytes([read_data[5], read_data[6]]);
|
||||
}
|
||||
Mct8316AVData::Four(val) => {
|
||||
*val = u32::from_be_bytes([read_data[5], read_data[6],
|
||||
read_data[7], read_data[8]]);
|
||||
*val = u32::from_be_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
|
||||
}
|
||||
Mct8316AVData::Eight(val) => {
|
||||
*val = u64::from_be_bytes([read_data[5], read_data[6],
|
||||
read_data[7], read_data[8],
|
||||
read_data[9], read_data[10],
|
||||
read_data[11], read_data[12]]);
|
||||
*val = u64::from_be_bytes([
|
||||
read_data[5],
|
||||
read_data[6],
|
||||
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: 'static,
|
||||
{
|
||||
#[allow(clippy::too_many_lines)]
|
||||
fn init(&mut self) -> Result<()> {
|
||||
trace!("Mct8316AVDriver::init(self: {self:?})");
|
||||
|
||||
@@ -309,7 +364,7 @@ where
|
||||
})?
|
||||
.set_constant_speed(ConstantSpeed {
|
||||
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_lower_limit: LowerPercentLimit::Percent10,
|
||||
mode: ClosedLoopMode::Disabled,
|
||||
@@ -362,10 +417,12 @@ where
|
||||
})?
|
||||
.set_fault_config1(FaultConfig1 {
|
||||
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_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,
|
||||
motor_lock_mode: LockMode::RecoverRetryTristated,
|
||||
lock_retry_time: LockRetryTime::Milliseconds5000,
|
||||
@@ -418,7 +475,8 @@ where
|
||||
overvoltage_protection_level: OvervoltageProtectionLevel::Volts32,
|
||||
overvoltage_protection_enable: true,
|
||||
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_level: OvercurrentProtectionLevel::Amps16,
|
||||
overcurrent_fault_mode: OvercurrentFaultMode::Latch,
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
use crate::hardware::mct8316a::Mct8316AVDriver;
|
||||
use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
|
||||
use crate::hardware::mct8316a::constant_power::ConstantPower;
|
||||
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::isd_config::IsdConfig;
|
||||
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::trap_config::{TrapConfig1, TrapConfig2};
|
||||
use crate::hardware::mct8316a::Mct8316AVDriver;
|
||||
use anyhow::Result;
|
||||
use embedded_hal::i2c::I2c;
|
||||
use log::trace;
|
||||
use std::any::type_name;
|
||||
use std::fmt::{Debug, Formatter};
|
||||
use std::thread::sleep;
|
||||
use std::time::Duration;
|
||||
|
||||
@@ -28,6 +32,26 @@ where
|
||||
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>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
@@ -36,9 +60,12 @@ where
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
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
|
||||
sleep(Duration::from_millis(100));
|
||||
|
||||
@@ -49,349 +76,470 @@ where
|
||||
}
|
||||
|
||||
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 =
|
||||
if isd_config.enable_isd { 0x40000000 } else { 0 }
|
||||
| if isd_config.enable_brake { 0x20000000 } else { 0 }
|
||||
| if isd_config.enable_high_impedance { 0x10000000 } else { 0 }
|
||||
| if isd_config.enable_reverse_drive { 0x08000000 } else { 0 }
|
||||
| if isd_config.enable_resynchronization { 0x04000000 } else { 0 }
|
||||
| if isd_config.enable_stationary_brake { 0x02000000 } 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;
|
||||
let expected_value = if isd_config.enable_isd {
|
||||
0x4000_0000
|
||||
} else {
|
||||
0
|
||||
} | if isd_config.enable_brake {
|
||||
0x2000_0000
|
||||
} else {
|
||||
0
|
||||
} | if isd_config.enable_high_impedance {
|
||||
0x1000_0000
|
||||
} else {
|
||||
0
|
||||
} | if isd_config.enable_reverse_drive {
|
||||
0x0800_0000
|
||||
} 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))
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
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 =
|
||||
((motor_startup1.motor_startup_method as u32) & 0x3) << 29
|
||||
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
|
||||
| ((motor_startup1.align_time as u32) & 0xF) << 21
|
||||
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
|
||||
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
|
||||
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
|
||||
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
|
||||
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
|
||||
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
|
||||
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
|
||||
let expected_value = ((motor_startup1.motor_startup_method as u32) & 0x3) << 29
|
||||
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
|
||||
| ((motor_startup1.align_time as u32) & 0xF) << 21
|
||||
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
|
||||
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
|
||||
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
|
||||
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
|
||||
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
|
||||
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
|
||||
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
|
||||
|
||||
self.assert_register(0x82, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
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 =
|
||||
((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_current_limit as u32) & 0xF) << 23
|
||||
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
|
||||
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
|
||||
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
|
||||
| if motor_startup2.auto_handoff_enable { 1u32 << 7 } else { 0u32 }
|
||||
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6
|
||||
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2;
|
||||
let expected_value = ((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_current_limit as u32) & 0xF) << 23
|
||||
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
|
||||
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
|
||||
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
|
||||
| if motor_startup2.auto_handoff_enable {
|
||||
1u32 << 7
|
||||
} else {
|
||||
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))
|
||||
}
|
||||
|
||||
// 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> {
|
||||
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 =
|
||||
((closed_loop1.commutation_mode as u32) & 0x2) << 29
|
||||
| ((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_rate as u32) & 0x1F) << 18
|
||||
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
|
||||
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
|
||||
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10
|
||||
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
|
||||
| (lead_angle as u32) << 1;
|
||||
let expected_value = ((closed_loop1.commutation_mode as u32) & 0x2) << 29
|
||||
| ((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_rate as u32) & 0x1F) << 18
|
||||
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
|
||||
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
|
||||
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10
|
||||
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
|
||||
| u32::from(lead_angle) << 1;
|
||||
|
||||
self.assert_register(0x86, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
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 =
|
||||
((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
|
||||
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
|
||||
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
|
||||
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
|
||||
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
|
||||
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
|
||||
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
|
||||
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
|
||||
| if closed_loop2.avs_enable { 1u32 << 7 } else { 0u32 }
|
||||
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
|
||||
let expected_value = ((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
|
||||
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
|
||||
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
|
||||
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
|
||||
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
|
||||
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
|
||||
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
|
||||
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
|
||||
| if closed_loop2.avs_enable {
|
||||
1u32 << 7
|
||||
} else {
|
||||
0u32
|
||||
}
|
||||
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
|
||||
|
||||
self.assert_register(0x88, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
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 =
|
||||
((closed_loop3.degauss_samples as u32) & 0x2) << 29
|
||||
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
|
||||
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
|
||||
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
|
||||
| ((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_high_threshold as u32) & 0x3) << 17
|
||||
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
|
||||
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
|
||||
| ((closed_loop3.commutation_method as u32) & 0x1) << 4
|
||||
| ((closed_loop3.degauss_window as u32) & 0x7) << 1
|
||||
| if closed_loop3.degauss_enable { 1 } else { 0u32 };
|
||||
let expected_value = ((closed_loop3.degauss_samples as u32) & 0x2) << 29
|
||||
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
|
||||
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
|
||||
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
|
||||
| ((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_high_threshold as u32) & 0x3) << 17
|
||||
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
|
||||
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
|
||||
| ((closed_loop3.commutation_method as u32) & 0x1) << 4
|
||||
| ((closed_loop3.degauss_window as u32) & 0x7) << 1
|
||||
| u32::from(closed_loop3.degauss_enable);
|
||||
|
||||
self.assert_register(0x8A, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
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 =
|
||||
if closed_loop4.wcomp_blanking_enable { 1u32 << 19 } else { 0u32 }
|
||||
| ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
|
||||
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
|
||||
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
|
||||
| if closed_loop4.dynamic_braking_current_enable { 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;
|
||||
let expected_value = if closed_loop4.wcomp_blanking_enable {
|
||||
1u32 << 19
|
||||
} else {
|
||||
0u32
|
||||
} | ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
|
||||
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
|
||||
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
|
||||
| if closed_loop4.dynamic_braking_current_enable {
|
||||
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))
|
||||
}
|
||||
|
||||
// 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> {
|
||||
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_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32;
|
||||
let speed_power_kp = (constant_speed.speed_power_kp * 10000f64)
|
||||
.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 =
|
||||
speed_power_kp << 20
|
||||
| speed_power_ki << 8
|
||||
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
|
||||
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
|
||||
| ((constant_speed.mode as u32) & 0x3) << 9;
|
||||
let expected_value = speed_power_kp << 20
|
||||
| speed_power_ki << 8
|
||||
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
|
||||
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
|
||||
| ((constant_speed.mode as u32) & 0x3) << 9;
|
||||
|
||||
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> {
|
||||
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_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
|
||||
let max_speed = (constant_power.max_speed * 16f64)
|
||||
.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 =
|
||||
max_speed << 15
|
||||
| if constant_power.dead_time_compensation_enable { 1u32 << 14 } else { 0u32 }
|
||||
| max_power << 4
|
||||
| ((constant_power.power_hysteresis as u32) & 0x3) << 2
|
||||
| ((constant_power.mode as u32) & 0x3) << 0;
|
||||
let expected_value = max_speed << 15
|
||||
| if constant_power.dead_time_compensation_enable {
|
||||
1u32 << 14
|
||||
} else {
|
||||
0u32
|
||||
}
|
||||
| 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))
|
||||
}
|
||||
|
||||
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 =
|
||||
((profile.steps[0] as u32) & 0x7) << 28
|
||||
| ((profile.steps[1] as u32) & 0x7) << 25
|
||||
| ((profile.steps[2] as u32) & 0x7) << 22
|
||||
| ((profile.steps[3] as u32) & 0x7) << 19
|
||||
| ((profile.steps[4] as u32) & 0x7) << 16
|
||||
| ((profile.steps[5] as u32) & 0x7) << 13
|
||||
| ((profile.steps[6] as u32) & 0x7) << 10
|
||||
| ((profile.steps[7] as u32) & 0x7) << 7;
|
||||
let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
|
||||
| ((profile.steps[1] as u32) & 0x7) << 25
|
||||
| ((profile.steps[2] as u32) & 0x7) << 22
|
||||
| ((profile.steps[3] as u32) & 0x7) << 19
|
||||
| ((profile.steps[4] as u32) & 0x7) << 16
|
||||
| ((profile.steps[5] as u32) & 0x7) << 13
|
||||
| ((profile.steps[6] as u32) & 0x7) << 10
|
||||
| ((profile.steps[7] as u32) & 0x7) << 7;
|
||||
|
||||
self.assert_register(0x96, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
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 =
|
||||
((profile.steps[0] as u32) & 0x7) << 28
|
||||
| ((profile.steps[1] as u32) & 0x7) << 25
|
||||
| ((profile.steps[2] as u32) & 0x7) << 22
|
||||
| ((profile.steps[3] as u32) & 0x7) << 19
|
||||
| ((profile.steps[4] as u32) & 0x7) << 16
|
||||
| ((profile.steps[5] as u32) & 0x7) << 13
|
||||
| ((profile.steps[6] as u32) & 0x7) << 10
|
||||
| ((profile.steps[7] as u32) & 0x7) << 7
|
||||
| ((profile.lead_angle as u32) & 0x3) << 5;
|
||||
let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
|
||||
| ((profile.steps[1] as u32) & 0x7) << 25
|
||||
| ((profile.steps[2] as u32) & 0x7) << 22
|
||||
| ((profile.steps[3] as u32) & 0x7) << 19
|
||||
| ((profile.steps[4] as u32) & 0x7) << 16
|
||||
| ((profile.steps[5] as u32) & 0x7) << 13
|
||||
| ((profile.steps[6] as u32) & 0x7) << 10
|
||||
| ((profile.steps[7] as u32) & 0x7) << 7
|
||||
| ((profile.lead_angle as u32) & 0x3) << 5;
|
||||
|
||||
self.assert_register(0x98, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
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 =
|
||||
((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
|
||||
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
|
||||
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
|
||||
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
|
||||
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
|
||||
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
|
||||
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0
|
||||
;
|
||||
let expected_value = ((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
|
||||
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
|
||||
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
|
||||
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
|
||||
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
|
||||
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
|
||||
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0;
|
||||
|
||||
self.assert_register(0x9A, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
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 =
|
||||
((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27
|
||||
| ((trap_config2.comparator_deglitch_time_microseconds as u32) & 0x7) << 24
|
||||
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
|
||||
let expected_value = (u32::from(trap_config2.blanking_time_microseconds) & 0xF) << 27
|
||||
| (u32::from(trap_config2.comparator_deglitch_time_microseconds) & 0x7) << 24
|
||||
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
|
||||
|
||||
self.assert_register(0x9C, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
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 =
|
||||
((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27
|
||||
| ((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_mode as u32) & 0xF) << 15
|
||||
| ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11
|
||||
| ((fault_config1.cycle_by_cycle_pwm_limit as u32) & 0x7) << 8
|
||||
| ((fault_config1.motor_lock_mode as u32) & 0xF) << 3
|
||||
| ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
|
||||
let expected_value = ((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27
|
||||
| ((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_mode as u32) & 0xF) << 15
|
||||
| ((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.motor_lock_mode as u32) & 0xF) << 3
|
||||
| ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
|
||||
|
||||
self.assert_register(0x92, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
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 =
|
||||
if fault_config2.lock_abnormal_speed_enable { 1u32 << 30 } else { 0u32 }
|
||||
| if fault_config2.lock_loss_of_sync_enable { 1u32 << 29 } else { 0u32 }
|
||||
| if fault_config2.lock_no_motor_enable { 1u32 << 28 } else { 0u32 }
|
||||
| ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24
|
||||
| ((fault_config2.loss_sync_times as u32) & 0x7) << 21
|
||||
| ((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;
|
||||
let expected_value = if fault_config2.lock_abnormal_speed_enable {
|
||||
1u32 << 30
|
||||
} else {
|
||||
0u32
|
||||
} | if fault_config2.lock_loss_of_sync_enable {
|
||||
1u32 << 29
|
||||
} else {
|
||||
0u32
|
||||
} | if fault_config2.lock_no_motor_enable {
|
||||
1u32 << 28
|
||||
} else {
|
||||
0u32
|
||||
} | ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24
|
||||
| ((fault_config2.loss_sync_times as u32) & 0x7) << 21
|
||||
| ((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))
|
||||
}
|
||||
|
||||
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 =
|
||||
((pin_config1.dacout1_address as u32) & 0xFFF) << 19
|
||||
| ((pin_config1.dacout2_address as u32) & 0xFFF) << 7
|
||||
| ((pin_config1.brake_input_config as u32) & 0x3) << 5
|
||||
| ((pin_config1.direction_input_config as u32) & 0x3) << 3
|
||||
| ((pin_config1.speed_input_config as u32) & 0x3) << 1;
|
||||
let expected_value = (u32::from(pin_config1.dacout1_address) & 0xFFF) << 19
|
||||
| (u32::from(pin_config1.dacout2_address) & 0xFFF) << 7
|
||||
| ((pin_config1.brake_input_config as u32) & 0x3) << 5
|
||||
| ((pin_config1.direction_input_config as u32) & 0x3) << 3
|
||||
| ((pin_config1.speed_input_config as u32) & 0x3) << 1;
|
||||
|
||||
self.assert_register(0xA4, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
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 =
|
||||
((pin_config2.pin36config as u32) & 0x3) << 29
|
||||
| ((pin_config2.pin37_38config as u32) & 0x1) << 27
|
||||
| ((pin_config2.sleep_time as u32) & 0x3) << 18
|
||||
| if pin_config2.enable_external_watchdog { 1u32 << 17 } else { 0u32 }
|
||||
| ((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;
|
||||
let expected_value = ((pin_config2.pin36config as u32) & 0x3) << 29
|
||||
| ((pin_config2.pin37_38config as u32) & 0x1) << 27
|
||||
| ((pin_config2.sleep_time as u32) & 0x3) << 18
|
||||
| if pin_config2.enable_external_watchdog {
|
||||
1u32 << 17
|
||||
} else {
|
||||
0u32
|
||||
}
|
||||
| ((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))
|
||||
}
|
||||
|
||||
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 =
|
||||
((device_config.max_frequency as u32) & 0x7FFF) << 16
|
||||
| if device_config.stl_enable { 1u32 << 15 } else { 0u32 }
|
||||
| if device_config.ssm_enable { 1u32 << 14 } 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;
|
||||
let expected_value = (u32::from(device_config.max_frequency) & 0x7FFF) << 16
|
||||
| if device_config.stl_enable {
|
||||
1u32 << 15
|
||||
} else {
|
||||
0u32
|
||||
}
|
||||
| if device_config.ssm_enable {
|
||||
1u32 << 14
|
||||
} 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))
|
||||
}
|
||||
|
||||
#[allow(clippy::identity_op)]
|
||||
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 =
|
||||
((gate_driver_config1.slew_rate as u32) & 0x3) << 26
|
||||
| ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19
|
||||
| if gate_driver_config1.overvoltage_protection_enable { 1u32 << 18 } else { 0u32 }
|
||||
| if gate_driver_config1.overtemperature_warning_enable { 1u32 << 16 } else { 0u32 }
|
||||
| ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12
|
||||
| ((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;
|
||||
let expected_value = ((gate_driver_config1.slew_rate as u32) & 0x3) << 26
|
||||
| ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19
|
||||
| if gate_driver_config1.overvoltage_protection_enable {
|
||||
1u32 << 18
|
||||
} else {
|
||||
0u32
|
||||
}
|
||||
| if gate_driver_config1.overtemperature_warning_enable {
|
||||
1u32 << 16
|
||||
} else {
|
||||
0u32
|
||||
}
|
||||
| ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12
|
||||
| ((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))
|
||||
}
|
||||
|
||||
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 =
|
||||
if gate_driver_config2.driver_delay_compensation_enable { 1u32 << 30 } else { 0u32 }
|
||||
| ((gate_driver_config2.target_delay as u32) & 0xF) << 26
|
||||
| ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25
|
||||
| if gate_driver_config2.buck_power_sequencing_disable { 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 };
|
||||
let expected_value = if gate_driver_config2.driver_delay_compensation_enable {
|
||||
1u32 << 30
|
||||
} else {
|
||||
0u32
|
||||
} | ((gate_driver_config2.target_delay as u32) & 0xF) << 26
|
||||
| ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25
|
||||
| if gate_driver_config2.buck_power_sequencing_disable {
|
||||
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))
|
||||
}
|
||||
|
||||
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)?;
|
||||
|
||||
@@ -412,9 +560,10 @@ where
|
||||
}
|
||||
|
||||
pub fn commit(self) -> Result<()> {
|
||||
trace!("Mct8316AVEeprom::commit()");
|
||||
trace!("Mct8316AVEeprom::commit(self: {self:?})");
|
||||
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
|
||||
sleep(Duration::from_millis(100));
|
||||
}
|
||||
|
||||
@@ -17,10 +17,12 @@ impl Default for FaultConfig1 {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
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_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,
|
||||
motor_lock_mode: LockMode::LatchFaultTristated,
|
||||
lock_retry_time: LockRetryTime::Milliseconds100,
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#![allow(dead_code)]
|
||||
|
||||
// This comes from the ICD
|
||||
#[allow(clippy::struct_excessive_bools)]
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct GateDriverConfig1 {
|
||||
pub slew_rate: SlewRate,
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#![allow(dead_code)]
|
||||
|
||||
// This comes from the ICD
|
||||
#[allow(clippy::struct_excessive_bools)]
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct IsdConfig {
|
||||
pub enable_isd: bool,
|
||||
|
||||
@@ -1,16 +1,16 @@
|
||||
mod closed_loop;
|
||||
mod constant_power;
|
||||
mod constant_speed;
|
||||
mod device_config;
|
||||
mod driver;
|
||||
mod eeprom;
|
||||
mod fault_config;
|
||||
mod gate_driver_config;
|
||||
mod isd_config;
|
||||
mod motor_startup;
|
||||
mod closed_loop;
|
||||
mod constant_speed;
|
||||
mod constant_power;
|
||||
mod phase_profile;
|
||||
mod trap_config;
|
||||
mod fault_config;
|
||||
mod pin_config;
|
||||
mod device_config;
|
||||
mod gate_driver_config;
|
||||
mod trap_config;
|
||||
|
||||
use anyhow::Result;
|
||||
|
||||
|
||||
@@ -35,7 +35,7 @@ impl Default for MotorStartup1 {
|
||||
pub enum MotorStartupMethod {
|
||||
Align = 0x0,
|
||||
DoubleAlign = 0x1,
|
||||
IPD = 0x2,
|
||||
Ipd = 0x2,
|
||||
SlowFirstCycle = 0x3,
|
||||
}
|
||||
|
||||
@@ -200,7 +200,6 @@ impl Default for MotorStartup2 {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub enum OpenLoopCurrentLimitMode {
|
||||
OpenLoopCurrentLimit = 0x0,
|
||||
@@ -358,6 +357,3 @@ pub enum MinimumDutyCycle {
|
||||
Percent25 = 0xE,
|
||||
Percent30 = 0xF,
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -135,4 +135,3 @@ impl Default for TrapConfig2 {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -2,12 +2,17 @@ use crate::hardware::mcp23017::Mcp23017;
|
||||
use crate::hardware::mct8316a::Mct8316a;
|
||||
use anyhow::Result;
|
||||
use embedded_hal::pwm::SetDutyCycle;
|
||||
use log::trace;
|
||||
use std::fmt::Debug;
|
||||
|
||||
pub trait Hardware {
|
||||
type Mcp23017<'a>: Mcp23017 + Send + Debug
|
||||
where
|
||||
Self: 'a;
|
||||
type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync;
|
||||
|
||||
fn new_mcp23017_a(&self) -> Result<impl Mcp23017 + Sync>;
|
||||
fn new_mcp23017_b(&self) -> Result<impl Mcp23017 + Sync>;
|
||||
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>>;
|
||||
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>>;
|
||||
|
||||
fn new_pwm0(&self) -> Result<Self::Pwm>;
|
||||
|
||||
@@ -21,24 +26,23 @@ mod raspi;
|
||||
|
||||
#[cfg(feature = "raspi")]
|
||||
pub fn initialize() -> Result<impl Hardware> {
|
||||
trace!("initialize()");
|
||||
raspi::RaspiHardware::new()
|
||||
}
|
||||
|
||||
#[cfg(not(feature = "raspi"))]
|
||||
#[allow(unreachable_code)]
|
||||
pub fn initialize() -> Result<impl Hardware> {
|
||||
panic!("Can not Initialize");
|
||||
|
||||
Ok(())
|
||||
trace!("initialize()");
|
||||
sim::SimHardware::new()
|
||||
}
|
||||
|
||||
#[cfg(not(feature = "raspi"))]
|
||||
mod bno085;
|
||||
#[cfg(not(feature = "raspi"))]
|
||||
mod imu;
|
||||
mod error;
|
||||
pub mod error;
|
||||
|
||||
pub mod mcp23017;
|
||||
mod mcp3208;
|
||||
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
26
flight/src/hardware/pin.rs
Normal 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);
|
||||
}
|
||||
@@ -1,10 +1,11 @@
|
||||
mod pwm;
|
||||
|
||||
use crate::hardware::mcp23017::{Mcp23017, Mcp23017Driver};
|
||||
use crate::hardware::Hardware;
|
||||
use crate::hardware::mcp3208::Mcp3208;
|
||||
use crate::hardware::mcp23017::Mcp23017Driver;
|
||||
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
|
||||
use crate::hardware::raspi::pwm::PwmWrapper;
|
||||
use crate::hardware::Hardware;
|
||||
use crate::hardware::sim::mct8316a::SimMct8316a;
|
||||
use anyhow::Result;
|
||||
use embedded_hal_bus::i2c::MutexDevice;
|
||||
use log::{debug, info, trace};
|
||||
@@ -18,10 +19,12 @@ use std::sync::Mutex;
|
||||
|
||||
const CLOCK_1MHZ: u32 = 1_000_000;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct RaspiHardware {
|
||||
_gpio: Gpio,
|
||||
i2c_bus: Mutex<I2c>,
|
||||
mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
|
||||
mct8316a: Mutex<SimMct8316a>,
|
||||
}
|
||||
|
||||
impl RaspiHardware {
|
||||
@@ -35,31 +38,44 @@ impl RaspiHardware {
|
||||
Ok(Self {
|
||||
_gpio: Gpio::new()?,
|
||||
i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
|
||||
mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new(
|
||||
Bus::Spi1,
|
||||
SlaveSelect::Ss1,
|
||||
CLOCK_1MHZ,
|
||||
Mode::Mode0,
|
||||
)?), 3.3f64).into(),
|
||||
mcp3208: Mcp3208::new(
|
||||
SimpleHalSpiDevice::new(Spi::new(
|
||||
Bus::Spi1,
|
||||
SlaveSelect::Ss1,
|
||||
CLOCK_1MHZ,
|
||||
Mode::Mode0,
|
||||
)?),
|
||||
3.3f64,
|
||||
)
|
||||
.into(),
|
||||
mct8316a: SimMct8316a::new().into(),
|
||||
// mct8316a: SimMct8316a::new().into(),
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl Hardware for RaspiHardware {
|
||||
type Mcp23017<'a> = Mcp23017Driver<MutexDevice<'a, I2c>>;
|
||||
type Pwm = PwmWrapper;
|
||||
|
||||
fn new_mcp23017_a(&self) -> Result<impl Mcp23017> {
|
||||
trace!("RaspiHardware::new_mcp23017_a()");
|
||||
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000))
|
||||
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
|
||||
trace!("RaspiHardware::new_mcp23017_a(self: {self:?})");
|
||||
Ok(Mcp23017Driver::new(
|
||||
MutexDevice::new(&self.i2c_bus),
|
||||
0b0100000,
|
||||
))
|
||||
}
|
||||
|
||||
fn new_mcp23017_b(&self) -> Result<impl Mcp23017> {
|
||||
trace!("RaspiHardware::new_mcp23017_b()");
|
||||
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001))
|
||||
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
|
||||
trace!("RaspiHardware::new_mcp23017_b(self: {self:?})");
|
||||
Ok(Mcp23017Driver::new(
|
||||
MutexDevice::new(&self.i2c_bus),
|
||||
0b0100001,
|
||||
))
|
||||
}
|
||||
|
||||
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
|
||||
// of the kernel where pwmchip for RPi5 was 2
|
||||
const PWMCHIP: u8 = 0;
|
||||
@@ -68,13 +84,15 @@ impl Hardware for RaspiHardware {
|
||||
}
|
||||
|
||||
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
|
||||
trace!("RaspiHardware::new_mct8316a()");
|
||||
Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000))
|
||||
trace!("RaspiHardware::new_mct8316a(self: {self:?})");
|
||||
Ok(Mct8316AVDriver::new(
|
||||
MutexDevice::new(&self.mct8316a),
|
||||
0b0000000,
|
||||
))
|
||||
}
|
||||
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 rpi_pal::pwm::Pwm;
|
||||
use std::fmt::{Display, Formatter};
|
||||
use std::time::Duration;
|
||||
|
||||
const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct PwmWrapper {
|
||||
pwm: Pwm,
|
||||
}
|
||||
@@ -16,41 +17,24 @@ impl PwmWrapper {
|
||||
pwm.set_period(PWM_PERIOD)?;
|
||||
pwm.enable()?;
|
||||
pwm.set_reset_on_drop(true);
|
||||
Ok(Self {
|
||||
pwm
|
||||
})
|
||||
Ok(Self { 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 {
|
||||
type Error = ErrorWrapper;
|
||||
type Error = WrappingError<rpi_pal::pwm::Error>;
|
||||
}
|
||||
|
||||
impl SetDutyCycle for PwmWrapper {
|
||||
fn max_duty_cycle(&self) -> u16 {
|
||||
trace!("PwmWrapper::max_duty_cycle()");
|
||||
trace!("PwmWrapper::max_duty_cycle(self: {self:?})");
|
||||
u16::MAX
|
||||
}
|
||||
|
||||
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
|
||||
trace!("PwmWrapper::set_duty_cycle(duty: {duty})");
|
||||
self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(ErrorWrapper)
|
||||
trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty: {duty})");
|
||||
self.pwm
|
||||
.set_duty_cycle((duty as f64) / (u16::MAX as f64))
|
||||
.map_err(WrappingError)
|
||||
}
|
||||
}
|
||||
|
||||
71
flight/src/hardware/sim/hardware.rs
Normal file
71
flight/src/hardware/sim/hardware.rs
Normal 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
57
flight/src/hardware/sim/mcp23017.rs
Normal 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
120
flight/src/hardware/sim/mct8316a.rs
Normal 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
12
flight/src/hardware/sim/mod.rs
Normal 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
49
flight/src/hardware/sim/pwm.rs
Normal 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(())
|
||||
}
|
||||
}
|
||||
@@ -1,30 +1,33 @@
|
||||
use crate::hardware::channelization::{MCP23017_A_LED, MCP23017_B_LED};
|
||||
use crate::hardware::initialize;
|
||||
use crate::hardware::mcp23017::Mcp23017OutputPin;
|
||||
use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task};
|
||||
use crate::hardware::mct8316a::Mct8316a;
|
||||
#![warn(clippy::all, clippy::pedantic)]
|
||||
use crate::comms::CommsTask;
|
||||
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 embedded_hal::digital::PinState;
|
||||
use embedded_hal::pwm::SetDutyCycle;
|
||||
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::thread;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::thread::sleep;
|
||||
use std::time::Duration;
|
||||
|
||||
mod hardware;
|
||||
|
||||
fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> Result<()> {
|
||||
ctrlc::set_handler(move || {
|
||||
info!("Shutdown Requested");
|
||||
flag.store(false, Ordering::Relaxed);
|
||||
})?;
|
||||
Ok(())
|
||||
fn new_shutdown_handler(running: &Arc<AtomicBool>) -> impl Fn(()) {
|
||||
let running = running.clone();
|
||||
move |()| running.store(false, Ordering::Relaxed)
|
||||
}
|
||||
|
||||
/// Run the flight software
|
||||
///
|
||||
/// # Errors
|
||||
/// An unrecoverable error had occurred
|
||||
pub fn run() -> Result<()> {
|
||||
info!(
|
||||
"Project Nautilus Flight Software {}",
|
||||
@@ -34,6 +37,8 @@ pub fn run() -> Result<()> {
|
||||
let running = Arc::new(AtomicBool::new(true));
|
||||
add_ctrlc_handler(running.clone())?;
|
||||
|
||||
let state_vector = StateVector::new();
|
||||
|
||||
let hal = initialize()?;
|
||||
|
||||
let mut mcp23017_a = hal.new_mcp23017_a()?;
|
||||
@@ -49,40 +54,50 @@ pub fn run() -> Result<()> {
|
||||
mcp23017_b.init()?;
|
||||
mct8316.init()?;
|
||||
|
||||
thread::scope(|scope| {
|
||||
// This will automatically set running to false when it drops
|
||||
// This means that if the main thread exits this scope, we will
|
||||
// shut down any side branches
|
||||
let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed));
|
||||
Scheduler::scope(running.clone(), |s| {
|
||||
let task_a = s.run_cyclic(
|
||||
"mcp23017-a-task",
|
||||
Mcp23017Task::new(mcp23017_a, &state_vector),
|
||||
10,
|
||||
)?;
|
||||
let a_id = task_a.get_id();
|
||||
|
||||
Mcp23017Task::new(&mcp23017_a, "mcp23017-a".into())
|
||||
.start(scope, &running, 10)?;
|
||||
let task_b = s.run_cyclic(
|
||||
"mcp23017-b-task",
|
||||
Mcp23017Task::new(mcp23017_b, &state_vector),
|
||||
10,
|
||||
)?;
|
||||
let b_id = task_b.get_id();
|
||||
|
||||
Mcp23017Task::new(&mcp23017_b, "mcp23017-b".into())
|
||||
.start(scope, &running, 10)?;
|
||||
let mut comms = CommsTask::new(15000, "nautilus-ground:14000")?;
|
||||
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)?;
|
||||
led_pin_a.set_state_on_drop(PinState::Low);
|
||||
let mut led_pin_b = mcp23017_b.new_output_pin(MCP23017_B_LED)?;
|
||||
led_pin_b.set_state_on_drop(PinState::Low);
|
||||
let sv = &state_vector;
|
||||
s.run_cyclic(
|
||||
"telemetry-producer",
|
||||
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,
|
||||
)?;
|
||||
|
||||
|
||||
loop {
|
||||
led_pin_a.set_state(PinState::High);
|
||||
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; };
|
||||
info!("Starting Main Loop");
|
||||
while running.load(Ordering::Relaxed) {
|
||||
sleep(Duration::from_millis(100));
|
||||
}
|
||||
|
||||
anyhow::Ok(())
|
||||
@@ -92,17 +107,21 @@ pub fn run() -> Result<()> {
|
||||
|
||||
// Explicitly drop these to allow the borrow checker to be sure that
|
||||
// dropping the hal is safe
|
||||
drop(mcp23017_a);
|
||||
drop(mcp23017_b);
|
||||
drop(pwm0);
|
||||
drop(mct8316);
|
||||
|
||||
drop(hal);
|
||||
|
||||
drop(state_vector);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test_utils;
|
||||
mod comms;
|
||||
mod data;
|
||||
mod on_drop;
|
||||
mod rcs;
|
||||
mod scheduler;
|
||||
mod state_vector;
|
||||
#[cfg(test)]
|
||||
mod test_utils;
|
||||
|
||||
@@ -1,15 +1,17 @@
|
||||
use crate::logger::setup_logger;
|
||||
use log::error;
|
||||
use nautilus_common::logger::setup_logger;
|
||||
use nautilus_flight::run;
|
||||
|
||||
mod logger;
|
||||
|
||||
fn main() {
|
||||
setup_logger().expect("Failed to setup logger");
|
||||
setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger");
|
||||
match run() {
|
||||
Ok(_) => {}
|
||||
Err(err) => {
|
||||
error!("An unhandled error occurred: {}\n\n{}", err, err.backtrace());
|
||||
error!(
|
||||
"An unhandled error occurred: {}\n\n{}",
|
||||
err,
|
||||
err.backtrace()
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,18 +1,18 @@
|
||||
use log::trace;
|
||||
use std::any::type_name;
|
||||
|
||||
pub struct OnDrop<F: FnMut()> {
|
||||
func: F,
|
||||
}
|
||||
|
||||
pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> {
|
||||
OnDrop {
|
||||
func
|
||||
}
|
||||
trace!("on_drop<F={}>()", type_name::<F>());
|
||||
OnDrop { func }
|
||||
}
|
||||
|
||||
impl<F: FnMut()> Drop for OnDrop<F> {
|
||||
fn drop(&mut self) {
|
||||
trace!("OnDrop::drop()");
|
||||
(self.func)()
|
||||
trace!("OnDrop<F={}>::drop()", type_name::<F>());
|
||||
(self.func)();
|
||||
}
|
||||
}
|
||||
|
||||
13
flight/src/rcs/mod.rs
Normal file
13
flight/src/rcs/mod.rs
Normal 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
149
flight/src/scheduler/mod.rs
Normal 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
165
flight/src/state_vector/mod.rs
Normal 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(())
|
||||
}
|
||||
}
|
||||
@@ -1,3 +1,4 @@
|
||||
#![allow(dead_code)]
|
||||
use std::mem::ManuallyDrop;
|
||||
use std::ops::{Deref, DerefMut};
|
||||
use std::panic;
|
||||
@@ -9,7 +10,7 @@ pub struct SilentDrop<T: panic::UnwindSafe> {
|
||||
impl<T: panic::UnwindSafe> SilentDrop<T> {
|
||||
pub fn new(inner: T) -> Self {
|
||||
Self {
|
||||
inner: ManuallyDrop::new(inner)
|
||||
inner: ManuallyDrop::new(inner),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -32,9 +33,7 @@ impl<T: panic::UnwindSafe> Drop for SilentDrop<T> {
|
||||
fn drop(&mut self) {
|
||||
let prev_hook = panic::take_hook();
|
||||
panic::set_hook(Box::new(|_| {}));
|
||||
let inner = unsafe {
|
||||
ManuallyDrop::take(&mut self.inner)
|
||||
};
|
||||
let inner = unsafe { ManuallyDrop::take(&mut self.inner) };
|
||||
let destroy_closure = || {
|
||||
drop(inner);
|
||||
};
|
||||
|
||||
@@ -4,3 +4,10 @@ version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[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
106
ground/src/lib.rs
Normal 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(())
|
||||
}
|
||||
@@ -1,3 +1,17 @@
|
||||
use log::error;
|
||||
use nautilus_common::logger::setup_logger;
|
||||
use nautilus_ground::run;
|
||||
|
||||
fn main() {
|
||||
println!("Hello, world!");
|
||||
setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger");
|
||||
match run() {
|
||||
Ok(_) => {}
|
||||
Err(err) => {
|
||||
error!(
|
||||
"An unhandled error occurred: {}\n\n{}",
|
||||
err,
|
||||
err.backtrace()
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user