free_flight_stabilization/pid/
cascade_angle.rs

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

free_flight_stabilization/pid/
cascade_angle.rs
