free_flight_stabilization/pid/
rate.rs

1// src/pid/rate.rs
2
3//! # Rate-Based PID Control Module
4//!
5//! This module provides a compute function and control data structure
6//! to perform rate-based PID (Proportional-Integral-Derivative) control
7//! calculations.
8
9use crate::Number;
10use piddiy::PidController;
11
12/// Control data for rate-based PID stabilization callback.
13#[derive(Debug, Clone, Copy, PartialEq, Default)]
14pub struct RateControlData<T> {
15    /// The current rate of change, typically reported by a gyro.
16    pub rate: T,
17    /// The time delta since the last computation.
18    pub dt: T,
19    /// The maximum allowed value for the integral term, used to prevent integral windup.
20    pub integral_limit: T,
21    /// Flag to reset the integral term, typically used when the controller is inactive.
22    pub reset_integral: bool,
23}
24
25/// Rate-based PID stabilization compute callback.
26pub fn compute_rate<T: Number>(
27    pid: &mut PidController<T, RateControlData<T>>,
28    data: RateControlData<T>,
29) -> (T, T, T) {
30    let error = pid.set_point - data.rate;
31    let integral = if !data.reset_integral {
32        (pid.integral + error * data.dt).clamp(-data.integral_limit, data.integral_limit)
33    } else {
34        T::zero()
35    };
36    let derivative = (error - pid.error) / data.dt;
37
38    (error, integral, derivative)
39}
40
41#[cfg(test)]
42mod tests {
43    use super::*;
44    use crate::test_utils::*;
45
46    /// Test that the integral term is clamped to the specified limit.
47    #[test]
48    fn test_pid_rate_integral_clamping() {
49        let mut pid = PidController::new();
50        pid.compute_fn(compute_rate)
51            .set_point(50.0)
52            .kp(1.0)
53            .ki(5.0)
54            .kd(0.1);
55        let data = RateControlData {
56            rate: 0.0,
57            dt: 1.0,
58            integral_limit: 100.0, // Integral should not exceed this value.
59            reset_integral: false,
60        };
61
62        // This would normally push integral way over 100 if not clamped
63        for _ in 0..10 {
64            let _ = pid.compute(data);
65        }
66
67        let (_, integral, _) = compute_rate(&mut pid, data);
68        assert!(
69            value_close(100.0, integral),
70            "Integral should be clamped to 100."
71        );
72    }
73
74    /// Test the behavior when the reset_integral flag is true.
75    #[test]
76    fn test_pid_rate_integral_reset() {
77        let mut pid = PidController::new();
78        pid.compute_fn(compute_rate)
79            .set_point(10.0)
80            .kp(1.0)
81            .ki(1.0)
82            .kd(0.1);
83        let data = RateControlData {
84            rate: 0.0,
85            dt: 1.0,
86            integral_limit: 100.0,
87            reset_integral: false,
88        };
89
90        // First compute without reset to build up the integral.
91        let (_, integral_first, _) = compute_rate(&mut pid, data);
92        let _ = pid.compute(data);
93
94        // Now compute with reset.
95        let data_reset = RateControlData {
96            reset_integral: true,
97            ..data
98        };
99        let (_, integral_reset, _) = compute_rate(&mut pid, data_reset);
100        let _ = pid.compute(data);
101
102        assert!(
103            value_close(integral_first, 10.0),
104            "Integral before reset should accumulate."
105        );
106        assert!(
107            value_close(integral_reset, 0.0),
108            "Integral after reset should be zero."
109        );
110    }
111
112    /// Test PID response with non-zero set point and zero rate.
113    #[test]
114    fn test_pid_rate_response() {
115        let mut pid = PidController::new();
116        pid.compute_fn(compute_rate)
117            .set_point(10.0)
118            .kp(1.0)
119            .ki(1.0)
120            .kd(1.0);
121        let data = RateControlData {
122            rate: 0.0,
123            dt: 1.0,
124            integral_limit: 100.0,
125            reset_integral: false,
126        };
127
128        let (mut error, mut integral, mut derivative) = compute_rate(&mut pid, data);
129        let mut output = pid.compute(data);
130
131        assert!(value_close(10.0, error), "Error should be 10.");
132        assert!(
133            value_close(10.0, integral),
134            "Integral should start to accumulate."
135        );
136        assert!(value_close(10.0, derivative), "Derivative should be 10.");
137        assert!(
138            value_close(30.0, output),
139            "Output should be the sum of terms."
140        );
141
142        // Call again to test accumulation
143        (error, integral, derivative) = compute_rate(&mut pid, data);
144        output = pid.compute(data);
145        assert!(value_close(10.0, error), "Error should be 10.");
146        assert!(
147            value_close(20.0, integral),
148            "Integral should accumulate to 20."
149        );
150        assert!(value_close(0.0, derivative), "Derivative should be zero.");
151        assert!(
152            value_close(30.0, output),
153            "Output should be the sum of terms."
154        );
155    }
156
157    /// Test PID specific response with non-zero values.
158    #[test]
159    fn test_pid_rate_specific_output() {
160        let mut pid = PidController::new();
161        pid.compute_fn(compute_rate)
162            .set_point(10.0)
163            .kp(1.0)
164            .ki(1.0)
165            .kd(1.0);
166        let data = RateControlData {
167            rate: 7.0,
168            dt: 1.0,
169            integral_limit: 100.0,
170            reset_integral: false,
171        };
172
173        let (error, integral, derivative) = compute_rate(&mut pid, data);
174        let output = pid.compute(data);
175
176        assert!(value_close(3.0, error), "Error should be 3.");
177        assert!(
178            value_close(3.0, integral),
179            "Integral should start to accumulate."
180        );
181        assert!(value_close(3.0, derivative), "Derivative should 3.");
182        assert!(
183            value_close(9.0, output),
184            "Output should be the sum of terms."
185        );
186
187        // Call again to test accumulation
188        let (_, integral_second, _) = compute_rate(&mut pid, data);
189        let _ = pid.compute(data);
190        assert!(
191            value_close(6.0, integral_second),
192            "Integral should accumulate to 6."
193        );
194    }
195
196    /// Test that PID computes zero output for zero error with zero initial conditions.
197    #[test]
198    fn test_pid_rate_zero_conditions() {
199        let mut pid = PidController::new();
200        pid.compute_fn(compute_rate)
201            .set_point(0.0)
202            .kp(1.0)
203            .ki(0.0)
204            .kd(0.0);
205        let data = RateControlData {
206            rate: 0.0,
207            dt: 1.0,
208            integral_limit: 10.0,
209            reset_integral: false,
210        };
211        let (error, integral, derivative) = compute_rate(&mut pid, data);
212        let output = pid.compute(data);
213
214        assert!(value_close(0.0, error), "Error should be zero.");
215        assert!(value_close(0.0, integral), "Integral should be zero.");
216        assert!(value_close(0.0, derivative), "Derivative should be zero.");
217        assert!(value_close(0.0, output), "Output should be zero.");
218    }
219}
free_flight_stabilization/pid/rate.rs

free_flight_stabilization/pid/
rate.rs
