bus -> device

appears to be twos complement

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,9 +7,4 @@ 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"}
-
-[features]
-default = ["ADIS1647x-1"]
-ADIS1647x-1 = []
-ADIS1647x-2 = []
-ADIS1647x-3 = []
+nalgebra = "0.31.4"

src/delta.rs

@@ -0,0 +1,82 @@
+use core::ops::AddAssign;
+
+use nalgebra::Vector3;
+
+#[derive(Default)]
+pub struct DeltaAngle {
+    angles: Vector3<i64>,
+    /// degrees per LSB
+    scale_factor: f32,
+}
+
+impl DeltaAngle {
+    pub(crate) fn new(scale_factor: f32, angles: Vector3<i32>) -> Self {
+        Self { 
+            angles: angles.map(|a| a as i64), 
+            scale_factor 
+        }
+    }
+
+    /// degrees
+    pub fn angles(&self) -> Vector3<f32> {
+        self.angles.map(|a| a as f32) * self.scale_factor
+    }
+}
+
+impl AddAssign for DeltaAngle {
+    /// uses the scale factor from the right delta
+    fn add_assign(&mut self, other: Self) {
+        self.angles += other.angles;
+        self.scale_factor = other.scale_factor;
+    }
+}
+
+#[derive(Default)]
+pub struct DeltaVelocity {
+    pub velocity: Vector3<i64>,
+    pub position: Vector3<i64>,
+    /// seconds between measurements, related to the decimation setting
+    delta_time: f32,
+    scale_factor: f32,
+}
+
+impl DeltaVelocity {
+    pub(crate) fn new(delta_time: f32, scale_factor: f32, velocities: Vector3<i32>) -> Self {
+        Self { 
+            velocity: velocities.map(|v| v as i64), 
+            position: Default::default(),
+            scale_factor,
+            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 * self.scale_factor * self.delta_time
+        })
+    }
+
+    /// metres per second
+    pub fn velocity(&self) -> Vector3<f32> {
+        self.velocity.map(|v| {
+            v as f32 * self.scale_factor
+        })
+    }
+}
+
+impl AddAssign for DeltaVelocity {
+    fn add_assign(&mut self, rhs: Self) {
+        self.integrate(rhs);
+    }
+}

src/lib.rs

@@ -1,25 +1,78 @@
 #![no_std]
-use embassy_embedded_hal::adapter::BlockingAsync;
-use embedded_hal_async::spi::{SpiBus, SpiDevice};
+use delta::{DeltaAngle, DeltaVelocity};
+use embedded_hal_async::spi::SpiDevice;
+use nalgebra::Vector3;
+use registers::*;
 
-#[cfg(feature = "ADIS1647x-1")]
-const DELTA_ANGLE_RANGE: f32 = 360.;
-#[cfg(feature = "ADIS1647x-2")]
-const DELTA_ANGLE_RANGE: f32 = 720.;
-#[cfg(feature = "ADIS1647x-3")]
-const DELTA_ANGLE_RANGE: f32 = 2160.;
+pub mod registers;
+pub mod delta;
 
-pub struct ADIS16475<T> where T: SpiBus {
+pub enum Model {
+    /// ADIS16475-1
+    ADIS16475_1,
+    /// ADIS16475-2
+    ADIS16475_2,
+    /// ADIS16475-3
+    ADIS16475_3,
+}
+
+impl Model {
+    /// m/s
+    const fn delta_velocity_range(&self) -> f32 { 100. }
+    const fn delta_velocity_per_lsb(&self) -> f32 { self.delta_velocity_range() / 32768.0 }
+
+    const fn delta_angle_range(&self) -> f32 {
+        match *self {
+            Model::ADIS16475_1 => 360.,
+            Model::ADIS16475_2 => 720.,
+            Model::ADIS16475_3 => 2160.,
+        }
+    }
+    const fn delta_angle_per_lsb(&self) -> f32 { self.delta_angle_range() / 32768.0 } 
+
+    /// construct from registers
+    ///
+    /// range: RANG_MDL (0x5E)
+    fn detect(range: u16) -> Self {
+        // isolate bits 2 and 3
+        let range = (range >> 2) & 0x3;
+        match range {
+            0b00 => Self::ADIS16475_1,
+            0b01 => Self::ADIS16475_2,
+            0b11 => Self::ADIS16475_3,
+            _ => unimplemented!(),
+        }
+    }
+}
+
+pub struct ADIS1647X<T> where T: SpiDevice {
     bus: T,
+    model: Model
 }
 
-impl<T: SpiBus> ADIS16475<T> {
-    pub fn new(bus: T) -> Self {
+impl<T: SpiDevice> ADIS1647X<T> {
+    pub async fn new(bus: T) -> Result<Self, T::Error> {
         //let bus = BlockingAsync::new(bus);
-        Self { bus }
+
+        let mut query_imu = Self { bus, model: Model::ADIS16475_1 };
+        let range = query_imu.read_u16(RANG_MDL).await?;
+
+        let model = Model::detect(range);
+        
+        Ok(Self::with_model(query_imu.take_bus(), model))
     }
 
-    async fn read_u16(&mut self, addr: u8) -> Result<u16, T::Error> {
+    fn take_bus(self) -> T {
+        self.bus
+    }
+
+    /// use a known model instead of detecting it
+    pub fn with_model(bus: T, model: Model) -> Self {
+        //let bus = BlockingAsync::new(bus);
+        Self { bus, model }
+    }
+
+    pub async fn read_u16(&mut self, addr: u8) -> Result<u16, T::Error> {
         let request = [addr & 0x7F, 0];
         self.bus.write(&request).await?;
         let mut dat = [0;2];
@@ -28,20 +81,20 @@ impl<T: SpiBus> ADIS16475<T> {
         Ok(u16::from_be_bytes(dat))
     }
 
-    async fn write_u8(&mut self, addr: u8, data: u8) -> Result<(), T::Error> {
+    pub async fn write_u8(&mut self, addr: u8, data: u8) -> Result<(), T::Error> {
         let request = [addr | 0x80, data];
         self.bus.write(&request).await?;
 
         Ok(())
     }
 
-    async fn read_u32(&mut self, addr: u8) -> Result<u32, T::Error> {
+    pub async fn read_u32(&mut self, addr: u8) -> Result<u32, T::Error> {
         let low = self.read_u16(addr).await? as u32;
         let high = self.read_u16(addr+2).await? as u32;
         Ok(high << 16 + low)
     }
 
-    async fn write_u16(&mut self, addr: u8, data: u16) -> Result<(), T::Error> {
+    pub async fn write_u16(&mut self, addr: u8, data: u16) -> Result<(), T::Error> {
         let data = u16::to_le_bytes(data);
         // TODO: do some soul searching
         self.write_u8(addr, data[0]).await?; // lower byte
@@ -49,8 +102,8 @@ impl<T: SpiBus> ADIS16475<T> {
         Ok(())
     }
 
-    /// efficiently reads multiple u32s by pipelining the spi bus
-    async fn read_u32_multi<const N: usize>(&mut self, addr: [u8;N]) -> Result<[u32;N], T::Error> {
+    /// efficiently reads multiple i32s by pipelining the spi bus
+    pub async fn read_i32_multi<const N: usize>(&mut self, addr: [u8;N]) -> Result<[i32;N], T::Error> {
         let read = |addr :u8| [(addr & 0x7F), 0];
 
         // first address
@@ -58,7 +111,7 @@ impl<T: SpiBus> ADIS16475<T> {
 
         let mut responses = [0; N];
 
-        let mut partial: [u16;2] = [0;2];
+        let mut partial: [i16;2] = [0;2];
         let mut partial_idx = 0;
 
         let mut buf: [u8;2] = [0;2];
@@ -67,20 +120,38 @@ impl<T: SpiBus> ADIS16475<T> {
             let addr = addr[i/2] + (i as u8 % 2) * 2;
             self.bus.transfer(&mut buf, &read(addr)).await?;
 
-            partial[partial_idx] = u16::from_be_bytes(buf);
+            partial[partial_idx] = i16::from_be_bytes(buf);
             partial_idx += 1;
 
             if partial_idx == 2 {
                 partial_idx = 0;
 
-                responses[i/2 -1] = (partial[1] as u32) << 16 + partial[0] as u32;
+                responses[i/2 -1] = (partial[1] as i32) << 16 + partial[0] as i32;
             }
         }
 
         // last address
         self.bus.read(&mut buf).await.unwrap();
-        responses[addr.len() - 1] = partial[0] as u32 + (u16::from_be_bytes(buf) as u32) << 16;
+        responses[addr.len() - 1] = partial[0] as i32 + (i16::from_be_bytes(buf) as i32) << 16;
 
         Ok(responses)
     }
+
+    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?;
+
+        let (angle, velocity) = (&data[..3], &data[3..]);
+
+        Ok((
+            DeltaAngle::new(self.model.delta_angle_per_lsb(), Vector3::from_row_slice(angle)),
+            DeltaVelocity::new(1.0/2000.0, self.model.delta_velocity_per_lsb(), Vector3::from_row_slice(velocity))
+        ))
+    }
 }

src/registers.rs

@@ -0,0 +1,39 @@
+//! register map, more information in 
+//! [Table 8 of the datasheet](https://www.analog.com/media/en/technical-documentation/data-sheets/adis16475.pdf#%5B%7B%22num%22%3A92%2C%22gen%22%3A0%7D%2C%7B%22name%22%3A%22XYZ%22%7D%2C52%2C693%2C0%5D)
+
+/// u16: Output, system error flags
+pub const DIAG_STAT: u8 = 0x02;
+
+pub const X_GYRO: u8 = 0x04;
+pub const Y_GYRO: u8 = 0x08;
+pub const Z_GYRO: u8 = 0x0C;
+
+pub const X_ACCL: u8 = 0x10;
+pub const Y_ACCL: u8 = 0x14;
+pub const Z_ACCL: u8 = 0x18;
+
+/// u16
+pub const TEMP: u8 = 0x1C;
+
+pub const X_DELTANG: u8 = 0x24;
+pub const Y_DELTANG: u8 = 0x28;
+pub const Z_DELTANG: u8 = 0x2C;
+
+pub const X_DELTVEL: u8 = 0x30;
+pub const Y_DELTVEL: u8 = 0x34;
+pub const Z_DELTVEL: u8 = 0x38;
+
+/// u16: Control, Bartlett window FIR filter
+pub const FLIT_CTRL: u8 = 0x5C;
+/// u16: Measurement range (model specific) identifier
+pub const RANG_MDL: u8 = 0x5E;
+/// u16: Control, input/output and other miscellaneous options
+pub const MSC_CTRL: u8 = 0x60;
+/// u16: Control, scale factor for input clock, pulse per second (PPS) mode
+pub const UP_SCALE: u8 = 0x62;
+/// u16: Control, decimation filter (output data rate)
+pub const DEC_RATE: u8 = 0x64;
+/// u16: Control, bias estimation period
+pub const NULL_CNFG: u8 = 0x66;
+/// u16: Control, global commands
+pub const GLOB_CMD: u8 = 0x68;