use nalgebra for DeltaAngle

src/delta.rs

@@ -1,4 +1,4 @@
-use core::ops::Add;
+use core::ops::AddAssign;
 
 use nalgebra::Vector3;
 
@@ -6,50 +6,30 @@ use crate::DELTA_VELOCITY_PER_LSB;
 
 #[derive(Default)]
 pub struct DeltaAngle {
-    pub x: i64,
+    angles: Vector3<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 {
+    pub fn new(scale_factor: f32, angles: Vector3<i32>) -> Self {
         Self { 
-            x: angles[0] as i64, 
+            angles: angles.map(|a| a as i64), 
-            y: angles[1] as i64, 
-            z: angles[2] as i64, 
             scale_factor 
         }
     }
 
     /// degrees
-    pub fn x(&self) -> f32 {
+    pub fn angles(&self) -> Vector3<f32> {
-        self.x as f32 * self.scale_factor
+        self.angles.map(|a| a 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 {
+impl AddAssign for DeltaAngle {
-    type Output = Self;
-
     /// uses the scale factor from the right delta
-    fn add(self, rhs: Self) -> Self::Output {
+    fn add_assign(&mut self, other: Self) {
-        Self {
+        self.angles += other.angles;
-            x: self.x + rhs.x,
+        self.scale_factor = other.scale_factor;
-            y: self.y + rhs.y,
-            z: self.z + rhs.z,
-            scale_factor: rhs.scale_factor,
-        }
     }
 }
 
@@ -63,7 +43,6 @@ pub struct DeltaVelocity {
 
 impl DeltaVelocity {
     pub fn new(delta_time: f32, velocities: Vector3<i32>) -> Self {
-        
         Self { 
             velocity: velocities.map(|v| v as i64), 
             position: Default::default(),
@@ -88,3 +67,9 @@ impl DeltaVelocity {
         })
     }
 }
+
+impl AddAssign for DeltaVelocity {
+    fn add_assign(&mut self, rhs: Self) {
+        self.integrate(rhs);
+    }
+}

src/lib.rs

@@ -109,7 +109,7 @@ impl<T: SpiBus> ADIS16475<T> {
         let (angle, velocity) = (&data[..3], &data[3..]);
 
         Ok((
-            DeltaAngle::new(DELTA_ANGLE_PER_LSB, angle),
+            DeltaAngle::new(DELTA_ANGLE_PER_LSB, Vector3::from_row_slice(angle)),
             DeltaVelocity::new(1.0/2000.0, Vector3::from_row_slice(velocity))
         ))
     }