embedded_flight_control/pid/
slew_limiter.rs

1use embedded_flight_core::filter::LowPassFilter;
2use num_traits::Float;
3
4/// Slew rate limiting filter.
5/// Used to prevent oscillation of a controller
6/// by modifying the controllers output based on a maximum slew rate
7
8pub struct SlewLimiter<const N: usize> {
9    slew_rate_max: f32,
10    slew_rate_tau: f32,
11    slew_filter: LowPassFilter<f32>,
12    pub output_slew_rate: f32,
13    _modifier_slew_rate: f32,
14    last_sample: f32,
15    _max_pos_slew_rate: f32,
16    _max_neg_slew_rate: f32,
17    _max_pos_slew_event_ms: u32,
18    _max_neg_slew_event_ms: u32,
19    _pos_event_index: u8,
20    _neg_event_index: u8,
21    _pos_event_ms: [u32; N],
22    _neg_event_ms: [u32; N],
23    _pos_event_stored: bool,
24    _neg_event_stored: bool,
25    window_ms: u32,
26    modifier_gain: f32,
27    cutoff_freq: f32,
28}
29
30impl<const N: usize> SlewLimiter<N> {
31    pub fn new(slew_rate_max: f32, slew_rate_tau: f32) -> Self {
32        Self {
33            slew_rate_max,
34            slew_rate_tau,
35            slew_filter: LowPassFilter::default(),
36            output_slew_rate: 0.,
37            _modifier_slew_rate: 0.,
38            last_sample: 0.,
39            _max_pos_slew_rate: 0.,
40            _max_neg_slew_rate: 0.,
41            _max_pos_slew_event_ms: 0,
42            _max_neg_slew_event_ms: 0,
43            _pos_event_index: 0,
44            _neg_event_index: 0,
45            _pos_event_ms: [0; N],
46            _neg_event_ms: [0; N],
47            _pos_event_stored: false,
48            _neg_event_stored: false,
49            window_ms: 300,
50            modifier_gain: 1.5,
51            cutoff_freq: 25.,
52        }
53    }
54    /// Apply the filter to a sample, returning multiplier between 0 and 1 to keep output within slew rate
55    pub fn modifier(&mut self, sample: f32, dt: f32, now_ms: u32) -> f32 {
56        if self.slew_rate_max <= 0. {
57            self.output_slew_rate = 0.;
58            return 1.;
59        }
60
61        // Calculate a low pass filtered slew rate
62        let slew_rate =
63            self.slew_filter
64                .filter((sample - self.last_sample) / dt, self.cutoff_freq, dt);
65        self.last_sample = sample;
66
67        let decay_alpha = dt.min(self.slew_rate_tau) / self.slew_rate_tau;
68
69        // Store a series of positive slew rate exceedance events
70        if !self._pos_event_stored && slew_rate > self.slew_rate_max {
71            if self._pos_event_index as usize >= N {
72                self._pos_event_index = 0;
73            }
74            self._pos_event_ms[self._pos_event_index as usize] = now_ms;
75            self._pos_event_index += 1;
76            self._pos_event_stored = true;
77            self._neg_event_stored = false;
78        }
79
80        // Store a series of negative slew rate exceedance events
81        if !self._neg_event_stored && slew_rate < -self.slew_rate_max {
82            if self._neg_event_index as usize >= N {
83                self._neg_event_index = 0;
84            }
85            self._neg_event_ms[self._neg_event_index as usize] = now_ms;
86            self._neg_event_index += 1;
87            self._neg_event_stored = true;
88            self._pos_event_stored = false;
89        }
90
91        // Find the oldest event time
92        let mut oldest_ms = now_ms;
93        for index in 0..N {
94            if self._pos_event_ms[index] < oldest_ms {
95                oldest_ms = self._pos_event_ms[index];
96            }
97            if self._neg_event_ms[index] < oldest_ms {
98                oldest_ms = self._neg_event_ms[index];
99            }
100        }
101
102        // Decay the peak positive and negative slew rate if they are outside the window
103        // Never drop PID gains below 10% of configured value
104        if slew_rate > self._max_pos_slew_rate {
105            self._max_pos_slew_rate = slew_rate.min(10. * self.slew_rate_max);
106            self._max_pos_slew_event_ms = now_ms;
107        } else if now_ms - self._max_pos_slew_event_ms > self.window_ms {
108            self._max_pos_slew_rate *= 1. - decay_alpha;
109        }
110
111        if slew_rate < -self._max_neg_slew_rate {
112            self._max_neg_slew_rate = -slew_rate.min(10. * self.slew_rate_max);
113            self._max_neg_slew_event_ms = now_ms;
114        } else if now_ms - self._max_neg_slew_event_ms > self.window_ms {
115            self._max_neg_slew_rate *= 1. - decay_alpha;
116        }
117
118        let raw_slew_rate = 0.5 * (self._max_pos_slew_rate + self._max_neg_slew_rate);
119
120        // Apply a further reduction when the oldest exceedance event falls outside the window rewuired for the
121        // specified number of exceedance events. This prevents spikes due to control mode changed, etc causing
122        // unwanted gain reduction and is only applied to the slew rate used for gain reduction
123        let mut modifier_input = raw_slew_rate;
124        if (now_ms - oldest_ms) as usize > (N + 1) * self.window_ms as usize {
125            let oldest_time_from_window = 0.001
126                * ((now_ms - oldest_ms) as f32 - ((N + 1) * self.window_ms as usize) as f32) as f32;
127            modifier_input *= (-oldest_time_from_window / self.slew_rate_tau).exp();
128        }
129
130        // Apply a filter to increases in slew rate only to reduce the effect of gusts and large controller
131        // setpoint changeschanges
132        let attack_alpha = (2. * decay_alpha).min(1.);
133
134        self._modifier_slew_rate =
135            (1. - attack_alpha) * self._modifier_slew_rate + attack_alpha * modifier_input;
136        self._modifier_slew_rate = self._modifier_slew_rate.min(modifier_input);
137
138        self.output_slew_rate =
139            (1. - attack_alpha) * self.output_slew_rate + attack_alpha * raw_slew_rate;
140        self.output_slew_rate = self.output_slew_rate.min(raw_slew_rate);
141
142        // Calculate the gain adjustment
143        if self._modifier_slew_rate > self.slew_rate_max {
144            self.slew_rate_max
145                / (self.slew_rate_max
146                    + self.modifier_gain * (self._modifier_slew_rate - self.slew_rate_max))
147        } else {
148            1.
149        }
150    }
151}
embedded_flight_control/pid/slew_limiter.rs

embedded_flight_control/pid/
slew_limiter.rs
