migrate to nalgebra

Cargo.toml

@@ -16,4 +16,4 @@ libm = "0.1.2"
 
 [dependencies.nalgebra]
 default-features = false
-version = "0.18"
+version = "0.18.0"

src/lib.rs

@@ -41,7 +41,7 @@
 //! 
 //! // get accelerometer data, scaled with sensitivity - averages across n readings (steps)
 //! let acc = mpu.get_acc_avg(Steps(5))?;
-//! 
+//! ```
 
 #![no_std]
 
@@ -49,7 +49,7 @@ pub mod registers;
 
 use crate::registers::Registers::*;
 use libm::{powf, atan2f, sqrtf};
-use nalgebra as na;
+use nalgebra::{Vector3, Vector2};
 use embedded_hal::{
     blocking::delay::DelayMs,
     blocking::i2c::{Write, WriteRead},
@@ -71,6 +71,16 @@ pub trait MutOps {
     fn scale(&mut self, n: u8);
 }
 
+/// Helpers for sensor readings
+pub trait Access<T> {
+    fn x(&self) -> T;
+    fn set_x(&mut self, val: T);
+    fn y(&self) -> T;
+    fn set_y(&mut self, val: T);
+    fn z(&self) -> T;
+    fn set_z(&mut self, val: T);
+}
+
 /// Trait for conversion from/to radians/degree
 pub trait UnitConv<T> {
     /// get radians from degree
@@ -101,45 +111,89 @@ impl UnitConv<f32> for f32 {
     }   
 }
 
+impl Access<f32> for Vector3<f32> {
+    fn x(&self) -> f32 {
+        self[0]
+    }
+
+    fn set_x(&mut self, val: f32) {
+        self[0] = val;
+    }
+
+    fn y(&self) -> f32 {
+        self[1]
+    }
+
+    fn set_y(&mut self, val: f32) {
+        self[1] = val;
+    }
+
+    fn z(&self) -> f32 {
+        self[2]
+    }
+
+    fn set_z(&mut self, val: f32) {
+        self[2] = val
+    }
+}
+
+impl Access<f32> for Vector2<f32> {
+    fn x(&self) -> f32 {
+        self[0]
+    }
+
+    fn set_x(&mut self, val: f32) {
+        self[0] = val;
+    }
+
+    fn y(&self) -> f32 {
+        self[1]
+    }
+
+    fn set_y(&mut self, val: f32) {
+        self[1] = val;
+    }
+
+    fn z(&self) -> f32 { 
+        -1.0
+    }
+
+    fn set_z(&mut self, _val: f32) {}
+}
+
 /// Used for bias calculation of chip in mpu::soft_calib
-#[derive(Default, Debug, Clone)]
+#[derive(Debug, Clone)]
 pub struct Bias {
-    /// accelerometer x axis bias
+    /// accelerometer axis bias
-    ax: f32,
+    acc: Vector3<f32>,
-    /// accelerometer y axis bias
-    ay: f32,
-    /// accelerometer z axis bias
-    az: f32,
     /// gyro x axis bias
-    gx: f32,
+    gyro: Vector3<f32>,
-    /// gyro y axis bias
-    gy: f32,
-    /// gyro z axis bias
-    gz: f32,
     /// temperature AVERAGE: can't get bias!
-    t: f32,
+    temp: f32,
+}
+
+impl Default for Bias {
+    fn default() -> Bias {
+        Bias {
+            acc: Vector3::<f32>::zeros(),
+            gyro: Vector3::<f32>::zeros(),
+            temp: 0.0,
+        }
+    }
 }
 
 impl Bias {
-    fn add(&mut self, acc: RotReading, gyro: RotReading, temp: f32) {
+    fn add(&mut self, acc: Vector3<f32>, gyro: Vector3<f32>, temp: f32) {
-        self.ax += acc.x;
+        self.acc += acc;
-        self.ay += acc.y;
+        self.gyro += gyro;
-        self.az += acc.z;
+        self.temp += temp; 
-        self.gx += gyro.x;
-        self.gy += gyro.y;
-        self.gz += gyro.z;
-        self.t += temp; 
     }
 
     fn scale(&mut self, n: u8) {
         let n = n as f32;
-        self.ax /= n;
+        self.acc /= n;
-        self.ay /= n;
+        self.gyro /= n;
-        self.az /= n;
+        self.temp /= n;
-        self.gx /= n;
-        self.gy /= n;
-        self.gz /= n;
-        self.t /= n;
     }
 }
 
@@ -147,108 +201,37 @@ impl Bias {
 pub type Variance = Bias;
 
 impl Variance {
-    fn add_diff(&mut self, acc_diff: (f32, f32, f32), gyro_diff: (f32, f32, f32), temp_diff: f32) {
+    fn add_diff(&mut self, acc_diff: Vector3<f32>, gyro_diff: Vector3<f32>, temp_diff: f32) {
-        self.ax += acc_diff.0;
+        self.acc += acc_diff;
-        self.ay += acc_diff.1;
+        self.gyro += gyro_diff;
-        self.az += acc_diff.2;
+        self.temp += temp_diff; 
-        self.gx += gyro_diff.0;
-        self.gy += gyro_diff.1;
-        self.gz += gyro_diff.2;
-        self.t += temp_diff; 
     }
 }
 
-/// Struct for rotation reading: gyro or accelerometer.
+/// Vector2 for Roll/Pitch Reading
-/// see indivisual type definitions
+impl UnitConv<Vector2<f32>> for Vector2<f32> {
-#[derive(Debug)]
+    fn to_rad(&self) -> Vector2<f32> {
-pub struct RotReading {
+        Vector2::<f32>::new(
-    pub x: f32,
+            self.x().to_rad(),
-    pub y: f32,
+            self.y().to_rad(),
-    pub z: f32,
+        )
-}
-
-/// Accelerometer reading
-pub type AccReading = RotReading;
-/// Gyro Reading
-pub type GyroReading = RotReading;
-
-impl RotReading {
-    fn new(x: f32, y: f32, z: f32) -> Self {
-        RotReading {
-            x, 
-            y, 
-            z,
-        }
-    }
-}
-
-impl MutOps for RotReading {
-    fn add(&mut self, operand: &Self) {
-        self.x += operand.x;
-        self.y += operand.y;
-        self.z += operand.z;
-    }
-
-    fn scale(&mut self, n: u8) {
-        let n = n as f32;
-        self.x /= n;
-        self.y /= n;
-        self.z /= n;
-    }
-}
-
-/// struct for Roll/Pitch Reading
-#[derive(Debug)]
-pub struct RPReading {
-    pub roll: f32,
-    pub pitch: f32,
-}
-
-impl RPReading {
-    fn new(roll: f32, pitch: f32) -> Self {
-        RPReading {
-            roll,
-            pitch,
-        }
-    }
-}
-
-impl MutOps for RPReading {
-    fn add(&mut self, operand: &Self) {
-        self.roll += operand.roll;
-        self.pitch += operand.pitch;
-    }
-
-    fn scale(&mut self, n: u8) {
-        let n = n as f32;
-        self.roll /= n;
-        self.pitch /= n;
-    }
-}
-
-impl UnitConv<RPReading> for RPReading {
-    fn to_rad(&self) -> RPReading {
-        RPReading {
-            roll: self.roll.to_rad(),
-            pitch: self.pitch.to_rad(),
-        }
     }
 
     fn to_rad_mut(&mut self) {
-        self.roll.to_rad_mut();
+        self[0].to_rad_mut();
-        self.pitch.to_rad_mut();
+        self[1].to_rad_mut();
     }
 
-    fn to_deg(&self) -> RPReading {
+    fn to_deg(&self) -> Vector2<f32> {
-        RPReading {
+        Vector2::<f32>::new(
-            roll: self.roll.to_deg(),
+            self.x().to_deg(),
-            pitch: self.pitch.to_deg(),
+            self.y().to_deg(),
-        }
+        )
     }
 
     fn to_deg_mut(&mut self) {
-        self.roll.to_deg_mut();
+        self[0].to_deg_mut();
-        self.pitch.to_deg_mut();
+        self[1].to_deg_mut();
     }
 }
 
@@ -381,7 +364,7 @@ where
         }   
 
         bias.scale(steps.0);
-        bias.az -= 1.0; // gravity compensation
+        bias.acc[2] -= 1.0; // gravity compensation
         self.bias = Some(bias);
 
         Ok(())
@@ -404,15 +387,19 @@ where
         let mut acc = self.get_acc()?;
         let mut gyro = self.get_gyro()?;
         let mut temp = self.get_temp()?;
-        let mut acc_diff: (f32, f32, f32); 
+        let mut acc_diff = Vector3::<f32>::zeros(); 
-        let mut gyro_diff: (f32, f32, f32);
+        let mut gyro_diff = Vector3::<f32>::zeros();
         let mut temp_diff: f32;
         let bias = self.bias.clone().unwrap();
 
         for _ in 0..iterations {
-           acc_diff = (powf(acc.x - bias.ax, 2.0), powf(acc.y - bias.ay, 2.0), powf(acc.z - bias.az, 2.0)); 
+           acc_diff.set_x(powf(acc.x() - bias.acc.x(), 2.0));
-           gyro_diff = (powf(gyro.x - bias.gx, 2.0), powf(gyro.y - bias.gy, 2.0), powf(gyro.z - bias.gz, 2.0));
+           acc_diff.set_y(powf(acc.y() - bias.acc.y(), 2.0));
-           temp_diff = powf(temp - bias.t, 2.0);
+           acc_diff.set_z(powf(acc.z() - bias.acc.z(), 2.0)); 
+           gyro_diff.set_x(powf(gyro.x() - bias.gyro.x(), 2.0));
+           gyro_diff.set_y(powf(gyro.y() - bias.gyro.y(), 2.0));
+           gyro_diff.set_z(powf(gyro.z() - bias.gyro.z(), 2.0));
+           temp_diff = powf(temp - bias.temp, 2.0);
            variance.add_diff(acc_diff, gyro_diff, temp_diff);
            acc = self.get_acc()?;
            gyro = self.get_gyro()?;
@@ -420,7 +407,7 @@ where
         }
 
         variance.scale(iterations-1);
-        variance.az -= 1.0; // gravity compensation
+        variance.acc[2] -= 1.0; // gravity compensation
         self.variance = Some(variance);
 
         Ok(())
@@ -433,20 +420,21 @@ where
 
     /// Roll and pitch estimation from raw accelerometer readings
     /// NOTE: no yaw! no magnetometer present on MPU6050
-    pub fn get_acc_angles(&mut self) -> Result<RPReading, Mpu6050Error<E>> {
+    pub fn get_acc_angles(&mut self) -> Result<Vector2<f32>, Mpu6050Error<E>> {
         let acc = self.get_acc()?;
-        let roll: f32 = atan2f(acc.y, sqrtf(powf(acc.x, 2.) + powf(acc.z, 2.)));
+        Ok(Vector2::<f32>::new(
-        let pitch: f32 = atan2f(-acc.x, sqrtf(powf(acc.y, 2.) + powf(acc.z, 2.)));
+            atan2f(acc.y(), sqrtf(powf(acc.x(), 2.) + powf(acc.z(), 2.))),
-        Ok(RPReading::new(roll, pitch))
+            atan2f(-acc.x(), sqrtf(powf(acc.y(), 2.) + powf(acc.z(), 2.)))
+        ))
     }
 
     /// Roll and pitch estimation from raw accelerometer - averaged across window readings
-    pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result<RPReading, Mpu6050Error<E>> {
+    pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result<Vector2<f32>, Mpu6050Error<E>> {
         let mut acc = self.get_acc_angles()?;
         for _ in 0..steps.0-1 {
-            acc.add(&self.get_acc_angles()?);
+            acc += self.get_acc_angles()?;
         }
-        acc.scale(steps.0);
+        acc /= steps.0 as f32;
         Ok(acc)
     }
 
@@ -465,68 +453,61 @@ where
     }
 
     /// Reads rotation (gyro/acc) from specified register
-    fn read_rot(&mut self, reg: u8) -> Result<RotReading, Mpu6050Error<E>> {
+    fn read_rot(&mut self, reg: u8) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         let mut buf: [u8; 6] = [0; 6];
         self.read_bytes(reg, &mut buf)?;
 
-        let xr = self.read_word_2c(&buf[0..2]);
+        Ok(Vector3::<f32>::new(
-        let yr = self.read_word_2c(&buf[2..4]);
+            self.read_word_2c(&buf[0..2]) as f32,
-        let zr = self.read_word_2c(&buf[4..6]);
+            self.read_word_2c(&buf[2..4]) as f32,
-        
+            self.read_word_2c(&buf[4..6]) as f32
-        Ok(RotReading::new(xr as f32, yr as f32, zr as f32)) // returning as f32 makes future calculations easier
+        ))
     }
 
     /// Accelerometer readings in m/s^2
-    pub fn get_acc(&mut self) -> Result<AccReading, Mpu6050Error<E>> {
+    pub fn get_acc(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         let mut acc = self.read_rot(ACC_REGX_H.addr())?;
         
-        acc.x /= self.acc_sensitivity;
+        acc /= self.acc_sensitivity;
-        acc.y /= self.acc_sensitivity;
-        acc.z /= self.acc_sensitivity;
 
         if let Some(ref bias) = self.bias { 
-            acc.x -= bias.ax;
+            acc -= bias.acc;
-            acc.y -= bias.ay;
-            acc.z -= bias.az;
         }
 
         Ok(acc)
     }
 
     /// Accelerometer readings in m/s^2 - averaged
-    pub fn get_acc_avg(&mut self, steps: Steps) -> Result<AccReading, Mpu6050Error<E>> {
+    pub fn get_acc_avg(&mut self, steps: Steps) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         let mut acc = self.get_acc()?;
         for _ in 0..steps.0-1 {
-            acc.add(&self.get_acc()?);
+            acc += self.get_acc()?;
         }
-        acc.scale(steps.0);
+        acc /= steps.0 as f32;
         Ok(acc)
     }
 
     /// Gyro readings in rad/s
-    pub fn get_gyro(&mut self) -> Result<GyroReading, Mpu6050Error<E>> {
+    pub fn get_gyro(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         let mut gyro = self.read_rot(GYRO_REGX_H.addr())?;
 
-        gyro.x *= PI / (180.0 * self.gyro_sensitivity);
+        gyro *= PI / (180.0 * self.gyro_sensitivity);
-        gyro.y *= PI / (180.0 * self.gyro_sensitivity);
-        gyro.z *= PI / (180.0 * self.gyro_sensitivity);
 
         if let Some(ref bias) = self.bias {
-            gyro.x -= bias.gx;
+            gyro -= bias.gyro;
-            gyro.y -= bias.gy;
-            gyro.z -= bias.gz;
         }
 
         Ok(gyro)
     }
     
     /// Gyro readings in rad/s
-    pub fn get_gyro_avg(&mut self, steps: Steps) -> Result<GyroReading, Mpu6050Error<E>> {
+    pub fn get_gyro_avg(&mut self, steps: Steps) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         let mut gyro = self.get_gyro()?;
         for _ in 0..steps.0-1 {
-            gyro.add(&self.get_gyro()?);
+            gyro += self.get_gyro()?;
         }
-        gyro.scale(steps.0);
+
+        gyro /= steps.0 as f32;
         Ok(gyro)
     }
 
@@ -575,6 +556,7 @@ where
 #[cfg(test)]
 mod tests {
     use super::*;
+    use nalgebra as na;
 
     #[test]
     fn test_unit_conv() {
@@ -590,4 +572,16 @@ mod tests {
         rad.to_deg_mut();
         assert_eq!(rad, 57.295776);
     }
+
+    #[test]
+    fn test_nalgebra() {
+        let mut v = Vector3::<f32>::new(1., 1., 1.);
+        let o = v.clone();
+        v *= 3.;
+        assert_eq!(Vector3::<f32>::new(3., 3., 3.), v);
+        v /= 3.;
+        assert_eq!(o, v);
+        v -= o;
+        assert_eq!(Vector3::<f32>::new(0., 0., 0.), v);
+    }
 }