From 1287701c0f1a48b722102dc82a673b0bee1bf05c Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 14 Mar 2025 20:54:12 -0400
Subject: [PATCH] detect model from range register

---
 src/lib.rs | 29 ++++++++++++++++++++++++++---
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index 868cb7c..0923803 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -30,6 +30,20 @@ impl Model {
         }
     }
     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 ADIS16475<T> where T: SpiBus {
@@ -38,10 +52,19 @@ pub struct ADIS16475<T> where T: SpiBus {
 }
 
 impl<T: SpiBus> ADIS16475<T> {
-    pub fn new(bus: T) -> Self {
+    pub async fn new(bus: T) -> Result<Self, T::Error> {
         //let bus = BlockingAsync::new(bus);
-        let model = Model::ADIS16475_1;
-        Self { bus, model }
+
+        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))
+    }
+
+    fn take_bus(self) -> T {
+        self.bus
     }
 
     /// use a known model instead of detecting it