imu_fusion/
fusion_gyr_offset_impl.rs

1use libm::{fabsf};
2use crate::{CUTOFF_FREQUENCY, FusionGyrOffset, FusionVector, THRESHOLD, TIMEOUT};
3
4impl FusionGyrOffset {
5    pub fn new(sample_rate: u32) -> Self {
6        Self {
7            filter_coefficient: 2.0f32 * core::f32::consts::PI * CUTOFF_FREQUENCY * (1.0f32 / sample_rate as f32),
8            gyroscope_offset: FusionVector::zero(),
9            timeout: TIMEOUT * sample_rate,
10            timer: 0,
11        }
12    }
13
14    pub fn update(&mut self, mut gyr: FusionVector) -> FusionVector {
15        // Subtract offset from gyroscope measurement
16        gyr = gyr - self.gyroscope_offset;
17        // Reset timer if gyroscope not stationary
18
19        if fabsf(gyr.x) > THRESHOLD || fabsf(gyr.y) > THRESHOLD || fabsf(gyr.z) > THRESHOLD {
20            self.timer = 0;
21            return gyr;
22        }
23
24        // Increment timer while gyroscope stationary
25        if self.timer < self.timeout {
26            self.timer += 1;
27            return gyr;
28        }
29        // Adjust offset if timer has elapsed
30        self.gyroscope_offset = self.gyroscope_offset + (gyr * self.filter_coefficient);
31        gyr
32    }
33}
imu_fusion/fusion_gyr_offset_impl.rs

imu_fusion/
fusion_gyr_offset_impl.rs
