embedded_flight_core/
filter.rs

1use num_traits::{Float, FloatConst, Zero};
2
3pub struct LowPassFilter<T> {
4    output: T,
5    alpha: T,
6}
7
8impl<T: Zero> Default for LowPassFilter<T> {
9    fn default() -> Self {
10        Self {
11            output: T::zero(),
12            alpha: T::zero(),
13        }
14    }
15}
16
17impl<T> LowPassFilter<T>
18where
19    T: Float + FloatConst,
20{
21    pub fn filter(&mut self, sample: T, cutoff_freq: T, dt: T) -> T {
22        if cutoff_freq <= T::zero() || dt <= T::zero() {
23            self.output = sample;
24            return self.output;
25        }
26
27        let rc = T::one() / (T::FRAC_2_PI() * cutoff_freq);
28        let alpha = dt / (dt + rc).min(T::one()).max(T::zero());
29        self.output = self.output + (sample - self.output) * alpha;
30        self.output
31    }
32
33    pub fn set_freq(&mut self, sample_freq: T, cutoff_freq: T) {
34        self.alpha = if sample_freq <= T::zero() {
35            T::one()
36        } else {
37            alpha(T::one() / sample_freq, cutoff_freq)
38        };
39    }
40}
41
42pub fn alpha<T: Float + FloatConst>(dt: T, cutoff_freq: T) -> T {
43    if cutoff_freq <= T::zero() || dt <= T::zero() {
44        return T::one();
45    }
46
47    let rc = T::one() / (T::FRAC_2_PI() * cutoff_freq);
48    dt / (dt + rc).min(T::one()).max(T::zero())
49}
50
51pub fn alpha_unchecked<T: Float + FloatConst>(dt: T, cutoff_freq: T) -> T {
52    let rc = T::one() / (T::FRAC_2_PI() * cutoff_freq);
53    dt / (dt + rc).min(T::one()).max(T::zero())
54}
embedded_flight_core/filter.rs

embedded_flight_core/
filter.rs
