From df2ba17dfd66c9dfe9320f5b12bccaab2b614f4b Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 14 Mar 2025 20:37:25 -0400
Subject: [PATCH] use model enum for scale parameters

---
 src/delta.rs     | 17 +++++++++++++----
 src/lib.rs       | 48 ++++++++++++++++++++++++++++++++++++++++--------
 src/registers.rs |  4 ++--
 3 files changed, 55 insertions(+), 14 deletions(-)

diff --git a/src/delta.rs b/src/delta.rs
index 6dc1213..ad31fb7 100644
--- a/src/delta.rs
+++ b/src/delta.rs
@@ -12,7 +12,7 @@ pub struct DeltaAngle {
 }
 
 impl DeltaAngle {
-    pub fn new(scale_factor: f32, angles: Vector3<i32>) -> Self {
+    pub(crate) fn new(scale_factor: f32, angles: Vector3<i32>) -> Self {
         Self { 
             angles: angles.map(|a| a as i64), 
             scale_factor 
@@ -39,14 +39,16 @@ pub struct DeltaVelocity {
     pub position: Vector3<i64>,
     /// seconds between measurements, related to the decimation setting
     delta_time: f32,
+    scale_factor: f32,
 }
 
 impl DeltaVelocity {
-    pub fn new(delta_time: f32, velocities: Vector3<i32>) -> Self {
+    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(),
-            delta_time 
+            scale_factor,
+            delta_time,
         }
     }
 
@@ -63,7 +65,14 @@ impl DeltaVelocity {
     /// metres
     pub fn position(&self) -> Vector3<f32> {
         self.position.map(|p| {
-            p as f32 * DELTA_VELOCITY_PER_LSB * self.delta_time
+            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
         })
     }
 }
diff --git a/src/lib.rs b/src/lib.rs
index 9d14752..f2b09a4 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -21,17 +21,49 @@ pub(crate) const DELTA_VELOCITY_PER_LSB: f32 = DELTA_VELOCITY_RANGE / 32768.0;
 pub mod registers;
 pub mod delta;
 
+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 } 
+}
+
 pub struct ADIS16475<T> where T: SpiBus {
     bus: T,
+    model: Model
 }
 
 impl<T: SpiBus> ADIS16475<T> {
     pub fn new(bus: T) -> Self {
         //let bus = BlockingAsync::new(bus);
-        Self { bus }
+        let model = Model::ADIS16475_1;
+        Self { bus, model }
     }
 
-    async fn read_u16(&mut self, addr: u8) -> Result<u16, T::Error> {
+    /// 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];
@@ -40,20 +72,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
@@ -62,7 +94,7 @@ impl<T: SpiBus> ADIS16475<T> {
     }
 
     /// efficiently reads multiple i32s by pipelining the spi bus
-    async fn read_i32_multi<const N: usize>(&mut self, addr: [u8;N]) -> Result<[i32;N], T::Error> {
+    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
@@ -109,8 +141,8 @@ impl<T: SpiBus> ADIS16475<T> {
         let (angle, velocity) = (&data[..3], &data[3..]);
 
         Ok((
-            DeltaAngle::new(DELTA_ANGLE_PER_LSB, Vector3::from_row_slice(angle)),
-            DeltaVelocity::new(1.0/2000.0, Vector3::from_row_slice(velocity))
+            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))
         ))
     }
 }
diff --git a/src/registers.rs b/src/registers.rs
index ec808d8..a05ee84 100644
--- a/src/registers.rs
+++ b/src/registers.rs
@@ -1,5 +1,5 @@
-//! register map, more information in Table 8 of the
-//! [datasheet](https://wiki.analog.com/resources/eval/user-guides/inertial-mems/imu/adis1647x-pcb?doc=adis16475.pdf#%5B%7B%22num%22%3A92%2C%22gen%22%3A0%7D%2C%7B%22name%22%3A%22XYZ%22%7D%2C52%2C693%2C0%5D)
+//! 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;