diff --git a/Cargo.lock b/Cargo.lock index 332e616..444522d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -9,8 +9,30 @@ dependencies = [ "embassy-embedded-hal", "embedded-hal 1.0.0", "embedded-hal-async", + "nalgebra", ] +[[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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" + +[[package]] +name = "bytemuck" +version = "1.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6b1fc10dbac614ebc03540c9dbd60e83887fda27794998c6528f1782047d540" + [[package]] name = "byteorder" version = "1.5.0" @@ -205,6 +227,43 @@ version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" +[[package]] +name = "matrixmultiply" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9380b911e3e96d10c1f415da0876389aaf1b56759054eeb0de7df940c456ba1a" +dependencies = [ + "autocfg", + "rawpointer", +] + +[[package]] +name = "nalgebra" +version = "0.31.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1" +dependencies = [ + "approx", + "matrixmultiply", + "nalgebra-macros", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + +[[package]] +name = "nalgebra-macros" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "01fcc0b8149b4632adc89ac3b7b31a12fb6099a0317a4eb2ebff574ef7de7218" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + [[package]] name = "nb" version = "0.1.3" @@ -220,6 +279,49 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" +[[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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +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-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "pin-project-lite" version = "0.2.16" @@ -232,14 +334,93 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" +[[package]] +name = "proc-macro2" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a31971752e70b8b2686d7e46ec17fb38dad4051d94024c88df49b667caea9c84" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.39" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1f1914ce909e1658d9907913b4b91947430c7d9be598b15a1912935b8c04801" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + +[[package]] +name = "safe_arch" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323" +dependencies = [ + "bytemuck", +] + +[[package]] +name = "simba" +version = "0.7.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2f3fd720c48c53cace224ae62bef1bbff363a70c68c4802a78b5cc6159618176" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", + "wide", +] + [[package]] name = "stable_deref_trait" version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" + [[package]] name = "void" version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "wide" +version = "0.7.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22" +dependencies = [ + "bytemuck", + "safe_arch", +] diff --git a/Cargo.toml b/Cargo.toml index 5ec5bdf..b8a3b85 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -7,6 +7,7 @@ edition = "2024" embedded-hal = "1.0.0" embedded-hal-async = "1.0.0" embassy-embedded-hal = {version = "0.3.0", git="https://github.com/embassy-rs/embassy", rev = "c39076724f052fed6781b056bb79c9fa576b87a3"} +nalgebra = "0.31.4" [features] default = ["ADIS1647x-1"] diff --git a/src/delta.rs b/src/delta.rs index 31cee82..a6ad839 100644 --- a/src/delta.rs +++ b/src/delta.rs @@ -1,5 +1,9 @@ use core::ops::Add; +use nalgebra::Vector3; + +use crate::DELTA_VELOCITY_PER_LSB; + #[derive(Default)] pub struct DeltaAngle { pub x: i64, @@ -48,3 +52,39 @@ impl Add for DeltaAngle { } } } + +#[derive(Default)] +pub struct DeltaVelocity { + pub velocity: Vector3, + pub position: Vector3, + /// seconds between measurements, related to the decimation setting + delta_time: f32, +} + +impl DeltaVelocity { + pub fn new(delta_time: f32, velocities: Vector3) -> Self { + + Self { + velocity: velocities.map(|v| v as i64), + position: Default::default(), + delta_time + } + } + + /// integrate another velocity into this one, keeping track of position + /// + /// create a new instance if you wish to change the decimation rate + pub fn integrate(&mut self, other: Self) { + self.delta_time = other.delta_time; + + self.velocity += other.velocity; + self.position += other.velocity; + } + + /// metres + pub fn position(&self) -> Vector3 { + self.position.map(|p| { + p as f32 * DELTA_VELOCITY_PER_LSB * self.delta_time + }) + } +} diff --git a/src/lib.rs b/src/lib.rs index cd8f51a..edd4a4a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,7 +1,8 @@ #![no_std] -use delta::DeltaAngle; +use delta::{DeltaAngle, DeltaVelocity}; use embassy_embedded_hal::adapter::BlockingAsync; use embedded_hal_async::spi::{SpiBus, SpiDevice}; +use nalgebra::Vector3; use registers::*; #[cfg(feature = "ADIS1647x-1")] @@ -13,6 +14,10 @@ const DELTA_ANGLE_RANGE: f32 = 2160.; const DELTA_ANGLE_PER_LSB: f32 = DELTA_ANGLE_RANGE / 32768.0; +/// m/s +const DELTA_VELOCITY_RANGE: f32 = 100.; +pub(crate) const DELTA_VELOCITY_PER_LSB: f32 = DELTA_VELOCITY_RANGE / 32768.0; + pub mod registers; pub mod delta; @@ -91,13 +96,21 @@ impl ADIS16475 { Ok(responses) } - pub async fn deltas(&mut self) -> Result { + pub async fn deltas(&mut self) -> Result<(DeltaAngle, DeltaVelocity), T::Error> { let data = self.read_i32_multi([ X_DELTANG, Y_DELTANG, Z_DELTANG, + X_DELTVEL, + Y_DELTVEL, + Z_DELTVEL, ]).await?; - Ok(DeltaAngle::new(DELTA_ANGLE_PER_LSB, data)) + let (angle, velocity) = (&data[..3], &data[3..]); + + Ok(( + DeltaAngle::new(DELTA_ANGLE_PER_LSB, angle), + DeltaVelocity::new(1.0/2000.0, Vector3::from_row_slice(velocity)) + )) } }