free_flight_stabilization/pid/
angle.rs

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

free_flight_stabilization/pid/
angle.rs
