diff --git a/src/delta.rs b/src/delta.rs index a6ad839..6dc1213 100644 --- a/src/delta.rs +++ b/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, - pub y: i64, - pub z: i64, + angles: Vector3, /// 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) -> Self { Self { - x: angles[0] as i64, - y: angles[1] as i64, - z: angles[2] as i64, + angles: angles.map(|a| a 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 + pub fn angles(&self) -> Vector3 { + self.angles.map(|a| a as f32) * self.scale_factor } } -impl Add for DeltaAngle { - type Output = Self; - +impl AddAssign for DeltaAngle { /// 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, - } + fn add_assign(&mut self, other: Self) { + self.angles += other.angles; + self.scale_factor = other.scale_factor; } } @@ -63,7 +43,6 @@ pub struct DeltaVelocity { impl DeltaVelocity { pub fn new(delta_time: f32, velocities: Vector3) -> 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); + } +} diff --git a/src/lib.rs b/src/lib.rs index edd4a4a..9d14752 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -109,7 +109,7 @@ impl ADIS16475 { 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)) )) }