embedded_flight_control/pid/
mod.rs

1use embedded_flight_core::filter::alpha;
2
3mod slew_limiter;
4pub use slew_limiter::SlewLimiter;
5
6pub struct Info {
7    pub target: f32,
8    pub actual: f32,
9    pub error: f32,
10    pub p: f32,
11    pub i: f32,
12    pub d: f32,
13    pub ff: f32,
14    pub d_mod: f32,
15    pub slew_rate: f32,
16    pub limit: bool,
17}
18
19pub struct PID {
20    pub reset_filter: bool,
21    pub target: f32,
22    pub error: f32,
23    pub derivative: f32,
24    // Timestep in seconds
25    pub dt: f32,
26    pub info: Info,
27    pub slew_limiter: SlewLimiter<2>,
28    pub slew_limit_scale: f32,
29    pub integrator: f32,
30    pub kp: f32,
31    pub ki: f32,
32    pub kd: f32,
33    pub ki_max: f32,
34    pub kff: f32,
35    pub filter_t_hz: f32,
36    pub filter_e_hz: f32,
37    pub filter_d_hz: f32,
38}
39
40impl PID {
41    pub fn new(
42        initial_p: f32,
43        initial_i: f32,
44        initial_d: f32,
45        initial_ff: f32,
46        initial_imax: f32,
47        initial_filt_T_hz: f32,
48        initial_filt_E_hz: f32,
49        initial_filt_D_hz: f32,
50        dt: f32,
51    ) -> Self {
52        Self::with_slew_rate(
53            initial_p,
54            initial_i,
55            initial_d,
56            initial_ff,
57            initial_imax,
58            initial_filt_T_hz,
59            initial_filt_E_hz,
60            initial_filt_D_hz,
61            dt,
62            0.,
63            1.,
64        )
65    }
66
67    pub fn with_slew_rate(
68        initial_p: f32,
69        initial_i: f32,
70        initial_d: f32,
71        initial_ff: f32,
72        initial_imax: f32,
73        initial_filt_T_hz: f32,
74        initial_filt_E_hz: f32,
75        initial_filt_D_hz: f32,
76        dt: f32,
77        initial_srmax: f32,
78        initial_srtau: f32,
79    ) -> Self {
80        Self {
81            reset_filter: true,
82            target: 0.,
83            error: 0.,
84            derivative: 0.,
85            dt,
86            info: Info {
87                target: 0.,
88                actual: 0.,
89                error: 0.,
90                p: 0.,
91                i: 0.,
92                d: 0.,
93                ff: 0.,
94                d_mod: 0.,
95                slew_rate: 0.,
96                limit: false,
97            },
98            slew_limiter: SlewLimiter::new(initial_srmax, initial_srtau),
99            slew_limit_scale: initial_srtau,
100            integrator: 0.,
101            kp: initial_p,
102            ki: initial_i,
103            kd: initial_d,
104            ki_max: initial_imax,
105            kff: initial_ff,
106            filter_t_hz: initial_filt_T_hz,
107            filter_e_hz: initial_filt_E_hz,
108            filter_d_hz: initial_filt_D_hz,
109        }
110    }
111}
112
113impl PID {
114    pub fn target_filter_alpha(&self) -> f32 {
115        self.alpha(self.filter_t_hz)
116    }
117
118    pub fn error_filter_alpha(&self) -> f32 {
119        self.alpha(self.filter_e_hz)
120    }
121
122    pub fn derivative_filter_alpha(&self) -> f32 {
123        self.alpha(self.filter_d_hz)
124    }
125
126    fn alpha(&self, cutoff_freq: f32) -> f32 {
127        alpha(self.dt, cutoff_freq)
128    }
129
130    pub fn feed_forward(&self) -> f32 {
131        self.target * self.kff
132    }
133
134    /// Update the integral part of this PID.
135    /// If the limit flag is set the integral is only allowed to shrink.
136    pub fn update_integral(&mut self, limit: bool) {
137        if self.ki != 0. && self.dt > -0. {
138            // Ensure that integrator can only be reduced if the output is saturated
139            if !limit
140                || (self.integrator >= 0. && self.error < 0.)
141                || self.integrator < 0. && self.error >= 0.
142            {
143                self.integrator += self.error * self.ki * self.dt;
144                self.integrator = self.integrator.max(-self.ki_max).min(self.ki_max);
145            }
146        } else {
147            self.integrator = 0.;
148        }
149        self.info.i = self.integrator;
150        self.info.limit = limit;
151    }
152
153    /// Update target and measured inputs to the PID controller and calculate output.
154    /// Target and error are filtered,
155    /// then derivative is calculated and filtered,
156    /// then the integral is then updated based on the setting of the limit flag
157    pub fn update(&mut self, target: f32, measurement: f32, limit: bool, now_ms: u32) -> f32 {
158        // don't process inf or NaN
159        if target.is_infinite() || measurement.is_infinite() {
160            return 0.;
161        }
162
163        // reset input filter to value received
164        if self.reset_filter {
165            self.reset_filter = false;
166            self.target = target;
167            self.error = self.target - measurement;
168            self.derivative = 0.;
169        } else {
170            let error_last = self.error;
171            self.target += self.target_filter_alpha() * (target - self.target);
172            self.error += self.error_filter_alpha() * ((self.target - measurement) - self.error);
173
174            // calculate and filter derivative
175            if self.dt > 0. {
176                let derivative = (self.error - error_last) / self.dt;
177                self.derivative += self.derivative_filter_alpha() * (derivative - self.derivative);
178            }
179        }
180
181        // update I term
182        self.update_integral(limit);
183
184        let mut p_out = self.error * self.kp;
185        let mut d_out = self.derivative * self.kd;
186
187        // calculate slew limit modifier for P+D
188        self.info.d_mod = self.slew_limiter.modifier(
189            (self.info.p + self.info.d) * self.slew_limit_scale,
190            self.dt,
191            now_ms,
192        );
193        self.info.slew_rate = self.slew_limiter.output_slew_rate;
194
195        p_out *= self.info.d_mod;
196        d_out *= self.info.d_mod;
197
198        self.info.target = self.target;
199        self.info.actual = measurement;
200        self.info.error = self.error;
201        self.info.p = p_out;
202        self.info.d = d_out;
203
204        p_out + self.integrator + d_out
205    }
206}
embedded_flight_control/pid/mod.rs

embedded_flight_control/pid/
mod.rs
