delta velocity

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",
+]

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"]

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<i64>,
+    pub position: Vector3<i64>,
+    /// seconds between measurements, related to the decimation setting
+    delta_time: f32,
+}
+
+impl DeltaVelocity {
+    pub fn new(delta_time: f32, velocities: Vector3<i32>) -> 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<f32> {
+        self.position.map(|p| {
+            p as f32 * DELTA_VELOCITY_PER_LSB * self.delta_time
+        })
+    }
+}

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<T: SpiBus> ADIS16475<T> {
         Ok(responses)
     }
 
-    pub async fn deltas(&mut self) -> Result<DeltaAngle, T::Error> {
+    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))
+        ))
     }
 }