From c1aa3c1f8bd2b6b08b6ffb2ec1f322c7ee251544 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Mon, 20 May 2019 16:00:24 +0200 Subject: [PATCH] migrate to nalgebra --- Cargo.toml | 2 +- src/lib.rs | 316 ++++++++++++++++++++++++++--------------------------- 2 files changed, 156 insertions(+), 162 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ed8c0d1..47892e5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,4 +16,4 @@ libm = "0.1.2" [dependencies.nalgebra] default-features = false -version = "0.18" +version = "0.18.0" diff --git a/src/lib.rs b/src/lib.rs index 977d871..f0bd123 100644 --- a/src/lib.rs +++ b/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 { + 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 { /// get radians from degree @@ -101,45 +111,89 @@ impl UnitConv for f32 { } } +impl Access for Vector3 { + 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 for Vector2 { + 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 - ax: f32, - /// accelerometer y axis bias - ay: f32, - /// accelerometer z axis bias - az: f32, + /// accelerometer axis bias + acc: Vector3, /// gyro x axis bias - gx: f32, - /// gyro y axis bias - gy: f32, - /// gyro z axis bias - gz: f32, + gyro: Vector3, /// temperature AVERAGE: can't get bias! - t: f32, + temp: f32, +} + +impl Default for Bias { + fn default() -> Bias { + Bias { + acc: Vector3::::zeros(), + gyro: Vector3::::zeros(), + temp: 0.0, + } + } } impl Bias { - fn add(&mut self, acc: RotReading, gyro: RotReading, temp: f32) { - self.ax += acc.x; - self.ay += acc.y; - self.az += acc.z; - self.gx += gyro.x; - self.gy += gyro.y; - self.gz += gyro.z; - self.t += temp; + fn add(&mut self, acc: Vector3, gyro: Vector3, temp: f32) { + self.acc += acc; + self.gyro += gyro; + self.temp += temp; } fn scale(&mut self, n: u8) { let n = n as f32; - self.ax /= n; - self.ay /= n; - self.az /= n; - self.gx /= n; - self.gy /= n; - self.gz /= n; - self.t /= n; + self.acc /= n; + self.gyro /= n; + self.temp /= 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) { - self.ax += acc_diff.0; - self.ay += acc_diff.1; - self.az += acc_diff.2; - self.gx += gyro_diff.0; - self.gy += gyro_diff.1; - self.gz += gyro_diff.2; - self.t += temp_diff; + fn add_diff(&mut self, acc_diff: Vector3, gyro_diff: Vector3, temp_diff: f32) { + self.acc += acc_diff; + self.gyro += gyro_diff; + self.temp += temp_diff; } } -/// Struct for rotation reading: gyro or accelerometer. -/// see indivisual type definitions -#[derive(Debug)] -pub struct RotReading { - pub x: f32, - pub y: f32, - 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 for RPReading { - fn to_rad(&self) -> RPReading { - RPReading { - roll: self.roll.to_rad(), - pitch: self.pitch.to_rad(), - } +/// Vector2 for Roll/Pitch Reading +impl UnitConv> for Vector2 { + fn to_rad(&self) -> Vector2 { + Vector2::::new( + self.x().to_rad(), + self.y().to_rad(), + ) } fn to_rad_mut(&mut self) { - self.roll.to_rad_mut(); - self.pitch.to_rad_mut(); + self[0].to_rad_mut(); + self[1].to_rad_mut(); } - fn to_deg(&self) -> RPReading { - RPReading { - roll: self.roll.to_deg(), - pitch: self.pitch.to_deg(), - } + fn to_deg(&self) -> Vector2 { + Vector2::::new( + self.x().to_deg(), + self.y().to_deg(), + ) } fn to_deg_mut(&mut self) { - self.roll.to_deg_mut(); - self.pitch.to_deg_mut(); + self[0].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 gyro_diff: (f32, f32, f32); + let mut acc_diff = Vector3::::zeros(); + let mut gyro_diff = Vector3::::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)); - gyro_diff = (powf(gyro.x - bias.gx, 2.0), powf(gyro.y - bias.gy, 2.0), powf(gyro.z - bias.gz, 2.0)); - temp_diff = powf(temp - bias.t, 2.0); + acc_diff.set_x(powf(acc.x() - bias.acc.x(), 2.0)); + acc_diff.set_y(powf(acc.y() - bias.acc.y(), 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> { + pub fn get_acc_angles(&mut self) -> Result, Mpu6050Error> { let acc = self.get_acc()?; - let roll: f32 = atan2f(acc.y, sqrtf(powf(acc.x, 2.) + powf(acc.z, 2.))); - let pitch: f32 = atan2f(-acc.x, sqrtf(powf(acc.y, 2.) + powf(acc.z, 2.))); - Ok(RPReading::new(roll, pitch)) + Ok(Vector2::::new( + atan2f(acc.y(), sqrtf(powf(acc.x(), 2.) + powf(acc.z(), 2.))), + 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> { + pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result, Mpu6050Error> { 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> { + fn read_rot(&mut self, reg: u8) -> Result, Mpu6050Error> { let mut buf: [u8; 6] = [0; 6]; self.read_bytes(reg, &mut buf)?; - let xr = self.read_word_2c(&buf[0..2]); - let yr = self.read_word_2c(&buf[2..4]); - let zr = self.read_word_2c(&buf[4..6]); - - Ok(RotReading::new(xr as f32, yr as f32, zr as f32)) // returning as f32 makes future calculations easier + Ok(Vector3::::new( + self.read_word_2c(&buf[0..2]) as f32, + self.read_word_2c(&buf[2..4]) as f32, + self.read_word_2c(&buf[4..6]) as f32 + )) } /// Accelerometer readings in m/s^2 - pub fn get_acc(&mut self) -> Result> { + pub fn get_acc(&mut self) -> Result, Mpu6050Error> { let mut acc = self.read_rot(ACC_REGX_H.addr())?; - acc.x /= self.acc_sensitivity; - acc.y /= self.acc_sensitivity; - acc.z /= self.acc_sensitivity; + acc /= self.acc_sensitivity; if let Some(ref bias) = self.bias { - acc.x -= bias.ax; - acc.y -= bias.ay; - acc.z -= bias.az; + acc -= bias.acc; } Ok(acc) } /// Accelerometer readings in m/s^2 - averaged - pub fn get_acc_avg(&mut self, steps: Steps) -> Result> { + pub fn get_acc_avg(&mut self, steps: Steps) -> Result, Mpu6050Error> { 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> { + pub fn get_gyro(&mut self) -> Result, Mpu6050Error> { let mut gyro = self.read_rot(GYRO_REGX_H.addr())?; - gyro.x *= PI / (180.0 * self.gyro_sensitivity); - gyro.y *= PI / (180.0 * self.gyro_sensitivity); - gyro.z *= PI / (180.0 * self.gyro_sensitivity); + gyro *= PI / (180.0 * self.gyro_sensitivity); if let Some(ref bias) = self.bias { - gyro.x -= bias.gx; - gyro.y -= bias.gy; - gyro.z -= bias.gz; + gyro -= bias.gyro; } Ok(gyro) } /// Gyro readings in rad/s - pub fn get_gyro_avg(&mut self, steps: Steps) -> Result> { + pub fn get_gyro_avg(&mut self, steps: Steps) -> Result, Mpu6050Error> { 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::::new(1., 1., 1.); + let o = v.clone(); + v *= 3.; + assert_eq!(Vector3::::new(3., 3., 3.), v); + v /= 3.; + assert_eq!(o, v); + v -= o; + assert_eq!(Vector3::::new(0., 0., 0.), v); + } }