steps struct to calrify filte size
This commit is contained in:
parent
2ed70201e5
commit
00cdf79b35
2 changed files with 20 additions and 21 deletions
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
25
src/lib.rs
25
src/lib.rs
|
@ -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)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue