rust_gnc/control/
pid.rs

1//! # PID Control Logic
2//! 
3//! This module implements a standard Proportional-Integral-Derivative (PID) 
4//! controller with integrated safety features for real-time systems.
5//!
6//! ### Implementation Details
7//! - **Parallel Form**: The output is the sum of $P$, $I$, and $D$ components.
8//! - **Anti-Windup**: Clamping is applied to the integral accumulator to 
9//!   prevent saturation during prolonged error states.
10//! - **Time Step Robustness**: Protects against $dt \le 0$ to ensure 
11//!   mathematical stability on embedded systems.
12
13/// Configuration parameters for a PID controller.
14/// 
15/// These gains determine the responsiveness and stability of the system.
16#[derive(Debug, Clone, Copy, PartialEq)]
17pub struct PidConfig {
18    /// Proportional gain ($K_p$). Immediate correction based on current error.
19    pub kp: f32,
20    /// Integral gain ($K_i$). Corrects for steady-state error over time.
21    pub ki: f32,
22    /// Derivative gain ($K_d$). Dampens oscillations by reacting to error rate.
23    pub kd: f32, 
24    /// Maximum absolute value for the integral accumulator (Anti-windup).
25    pub max_integral: f32, 
26}
27
28/// A stateful PID controller.
29pub struct PidController {
30    config: PidConfig,
31    /// The accumulated error sum (Integral term).
32    integral: f32,
33    /// The error value from the previous time step (for Derivative calculation).
34    last_error: f32,
35}
36
37/// A stateful PID controller.
38impl PidController {
39    /// Initializes a new controller with the provided gains and zeroed state.
40    pub fn new(config: PidConfig) -> Self {
41        Self {
42            config,
43            integral: 0.0,
44            last_error: 0.0,
45        }
46    }
47
48    /// Calculates the corrective signal based on a setpoint and measurement.
49    /// 
50    /// ### Parameters
51    /// * `setpoint` - The desired value (Target).
52    /// * `measurement` - The current estimated value (Feedback).
53    /// * `dt` - Time delta since the last update in seconds.
54    pub fn update(&mut self, setpoint: f32, measurement: f32, dt: f32) -> f32 {
55        let error = setpoint - measurement;
56        self.update_with_error(error, dt)
57    }
58
59    /// Primary calculation engine for the PID signal.
60    /// 
61    /// This method performs the following:
62    /// 1. **Proportional**: $K_p \times error$
63    /// 2. **Integral**: Accumulates $error \times dt$, clamped by `max_integral`.
64    /// 3. **Derivative**: $K_d \times \frac{\Delta error}{dt}$
65    pub fn update_with_error(&mut self, error: f32, dt: f32) -> f32 {
66
67        let proportional = self.config.kp * error;
68
69        if dt <= 0.0 {
70            return proportional; // Avoid division by zero or negative time step
71        }
72
73        // Integral Term with Anti-Windup Clamping
74        // This prevents the drone from "overshooting" aggressively after being held.
75        self.integral += error * dt;
76        self.integral = self.integral.clamp(-self.config.max_integral, self.config.max_integral);
77        let integral = self.config.ki * self.integral;
78
79        let derivative = self.config.kd * (error - self.last_error) / dt;
80
81        self.last_error = error;
82
83        proportional + integral + derivative
84    }
85
86    /// Resets the controller's internal memory (Integral and Last Error).
87    /// 
88    /// **CRITICAL**: This should be called whenever the controller is 
89    /// re-engaged (Armed) to prevent "jumpy" initial behavior from stale data.
90    pub fn reset(&mut self) {
91        self.integral = 0.0;
92        self.last_error = 0.0;
93    }
94}
95
96#[cfg(test)]
97mod tests {
98    use super::*;
99    
100    #[test]
101    fn test_pid_anti_windup() {
102        let config = PidConfig { kp: 1.0, ki: 1.0, kd: 0.0, max_integral: 5.0 };
103        let mut pid = PidController::new(config);
104
105        // Simulate a massive error over a long time (50 seconds)
106        // Without clamping, integral would be 50.0. With clamping, it should be 5.0.
107        for _ in 0..50 {
108            pid.update(10.0, 0.0, 1.0);
109        }
110        
111        // Final output should be P (10) + I (5) = 15.0
112        let output = pid.update(10.0, 0.0, 1.0);
113        assert_eq!(output, 15.0);
114    }
115}
rust_gnc/control/pid.rs

rust_gnc/control/
pid.rs
