1
Fork 0

steps struct to calrify filte size

This commit is contained in:
juliangaal 2019-04-24 16:00:48 +02:00
parent 2ed70201e5
commit 00cdf79b35
2 changed files with 20 additions and 21 deletions

View file

@ -21,32 +21,32 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let acc = mpu.get_acc_angles()?; let acc = mpu.get_acc_angles()?;
println!("r/p: {:?}", acc); println!("r/p: {:?}", acc);
// get roll and pitch estimate - averaged accross n readings (mask) // get roll and pitch estimate - averaged accross n readings (steps)
let acc = mpu.get_acc_angles_avg(Mask(5))?; let acc = mpu.get_acc_angles_avg(Steps(5))?;
println!("r/p avg: {:?}", acc); println!("r/p avg: {:?}", acc);
// get temp // get temp
let temp = mpu.get_temp()?; let temp = mpu.get_temp()?;
println!("temp: {}c", temp); println!("temp: {}c", temp);
// get temp - averages across n readings (mask) // get temp - averages across n readings (steps)
let temp = mpu.get_temp_avg(Mask(5))?; let temp = mpu.get_temp_avg(Steps(5))?;
println!("temp avg: {}c", temp); println!("temp avg: {}c", temp);
// get gyro data, scaled with sensitivity // get gyro data, scaled with sensitivity
let gyro = mpu.get_gyro()?; let gyro = mpu.get_gyro()?;
println!("gyro: {:?}", gyro); println!("gyro: {:?}", gyro);
// get gyro data, scaled with sensitivity - averaged across n readings (mask) // get gyro data, scaled with sensitivity - averaged across n readings (steps)
let gyro = mpu.get_gyro_avg(Mask(5))?; let gyro = mpu.get_gyro_avg(Steps(5))?;
println!("gyro avg: {:?}", gyro); println!("gyro avg: {:?}", gyro);
// get accelerometer data, scaled with sensitivity // get accelerometer data, scaled with sensitivity
let acc = mpu.get_acc()?; let acc = mpu.get_acc()?;
println!("acc: {:?}", acc); println!("acc: {:?}", acc);
// get accelerometer data, scaled with sensitivity - averages across n readings (mask) // get accelerometer data, scaled with sensitivity - averages across n readings (steps)
let acc = mpu.get_acc_avg(Mask(5))?; let acc = mpu.get_acc_avg(Steps(5))?;
println!("acc avg: {:?}", acc); println!("acc avg: {:?}", acc);
} }
} }

View file

@ -141,7 +141,6 @@ impl MutOps for RPReading {
/// Helper struct used as number of steps for filtering /// Helper struct used as number of steps for filtering
pub struct Steps(pub u8); pub struct Steps(pub u8);
pub type Mask = Steps;
// Helper struct to convert Sensor measurement range to appropriate values defined in datasheet // Helper struct to convert Sensor measurement range to appropriate values defined in datasheet
struct Sensitivity(f32); struct Sensitivity(f32);
@ -329,12 +328,12 @@ where
} }
/// Roll and pitch estimation from raw accelerometer - averaged across window readings /// Roll and pitch estimation from raw accelerometer - averaged across window readings
pub fn get_acc_angles_avg(&mut self, mask: Mask) -> Result<RPReading, Mpu6050Error<E>> { pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result<RPReading, Mpu6050Error<E>> {
let mut acc = self.get_acc_angles()?; let mut acc = self.get_acc_angles()?;
for _ in 0..mask.0-1 { for _ in 0..steps.0-1 {
acc.add(&self.get_acc_angles()?); acc.add(&self.get_acc_angles()?);
} }
acc.scale(mask.0); acc.scale(steps.0);
Ok(acc) Ok(acc)
} }
@ -382,12 +381,12 @@ where
} }
/// Accelerometer readings in m/s^2 - averaged /// Accelerometer readings in m/s^2 - averaged
pub fn get_acc_avg(&mut self, mask: Mask) -> Result<RotReading, Mpu6050Error<E>> { pub fn get_acc_avg(&mut self, steps: Steps) -> Result<RotReading, Mpu6050Error<E>> {
let mut acc = self.get_acc()?; let mut acc = self.get_acc()?;
for _ in 0..mask.0-1 { for _ in 0..steps.0-1 {
acc.add(&self.get_acc()?); acc.add(&self.get_acc()?);
} }
acc.scale(mask.0); acc.scale(steps.0);
Ok(acc) Ok(acc)
} }
@ -409,12 +408,12 @@ where
} }
/// Gyro readings in rad/s /// Gyro readings in rad/s
pub fn get_gyro_avg(&mut self, mask: Mask) -> Result<RotReading, Mpu6050Error<E>> { pub fn get_gyro_avg(&mut self, steps: Steps) -> Result<RotReading, Mpu6050Error<E>> {
let mut gyro = self.get_gyro()?; let mut gyro = self.get_gyro()?;
for _ in 0..mask.0-1 { for _ in 0..steps.0-1 {
gyro.add(&self.get_gyro()?); gyro.add(&self.get_gyro()?);
} }
gyro.scale(mask.0); gyro.scale(steps.0);
Ok(gyro) Ok(gyro)
} }
@ -427,12 +426,12 @@ where
Ok((raw_temp / 340.) + 36.53) Ok((raw_temp / 340.) + 36.53)
} }
pub fn get_temp_avg(&mut self, mask: Mask) -> Result<f32, Mpu6050Error<E>> { pub fn get_temp_avg(&mut self, steps: Steps) -> Result<f32, Mpu6050Error<E>> {
let mut temp = self.get_temp()?; let mut temp = self.get_temp()?;
for _ in 0..mask.0-1 { for _ in 0..steps.0-1 {
temp += self.get_temp()?; temp += self.get_temp()?;
} }
temp /= mask.0 as f32; temp /= steps.0 as f32;
Ok(temp) Ok(temp)
} }