From f7402e3de49403d8686e41331f8333a06b7a88e8 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 14 Mar 2025 17:21:49 -0400
Subject: [PATCH] delta angle

---
 src/delta.rs | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++
 src/lib.rs   | 14 ++++++++++++++
 2 files changed, 64 insertions(+)
 create mode 100644 src/delta.rs

diff --git a/src/delta.rs b/src/delta.rs
new file mode 100644
index 0000000..31cee82
--- /dev/null
+++ b/src/delta.rs
@@ -0,0 +1,50 @@
+use core::ops::Add;
+
+#[derive(Default)]
+pub struct DeltaAngle {
+    pub x: i64,
+    pub y: i64,
+    pub z: i64,
+    /// degrees per LSB
+    scale_factor: f32,
+}
+
+impl DeltaAngle {
+    pub fn new(scale_factor: f32, angles: [i32;3]) -> Self {
+        Self { 
+            x: angles[0] as i64, 
+            y: angles[1] as i64, 
+            z: angles[2] as i64, 
+            scale_factor 
+        }
+    }
+
+    /// degrees
+    pub fn x(&self) -> f32 {
+        self.x as f32 * self.scale_factor
+    }
+
+    /// degrees
+    pub fn y(&self) -> f32 {
+        self.y as f32 * self.scale_factor
+    }
+
+    /// degrees
+    pub fn z(&self) -> f32 {
+        self.z as f32 * self.scale_factor
+    }
+}
+
+impl Add for DeltaAngle {
+    type Output = Self;
+
+    /// uses the scale factor from the right delta
+    fn add(self, rhs: Self) -> Self::Output {
+        Self {
+            x: self.x + rhs.x,
+            y: self.y + rhs.y,
+            z: self.z + rhs.z,
+            scale_factor: rhs.scale_factor,
+        }
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index de043dd..cd8f51a 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,4 +1,5 @@
 #![no_std]
+use delta::DeltaAngle;
 use embassy_embedded_hal::adapter::BlockingAsync;
 use embedded_hal_async::spi::{SpiBus, SpiDevice};
 use registers::*;
@@ -10,7 +11,10 @@ const DELTA_ANGLE_RANGE: f32 = 720.;
 #[cfg(feature = "ADIS1647x-3")]
 const DELTA_ANGLE_RANGE: f32 = 2160.;
 
+const DELTA_ANGLE_PER_LSB: f32 = DELTA_ANGLE_RANGE / 32768.0;
+
 pub mod registers;
+pub mod delta;
 
 pub struct ADIS16475<T> where T: SpiBus {
     bus: T,
@@ -86,4 +90,14 @@ impl<T: SpiBus> ADIS16475<T> {
 
         Ok(responses)
     }
+
+    pub async fn deltas(&mut self) -> Result<DeltaAngle, T::Error> {
+        let data = self.read_i32_multi([
+            X_DELTANG,
+            Y_DELTANG,
+            Z_DELTANG,
+        ]).await?;
+
+        Ok(DeltaAngle::new(DELTA_ANGLE_PER_LSB, data))
+    }
 }