mecha10_behavior_runtime/control/

pid.rs

//! Generic PID (proportional-integral-derivative) controller.
//!
//! Used by flight-stabilization action nodes (`actions::flight::TakeoffNode`/`LandNode`) to turn
//! a setpoint/measurement error (e.g. target vs. current altitude) into a bounded control output
//! (e.g. per-rotor thrust), but it's deliberately domain-agnostic - nothing here references
//! altitude, thrust, or any mecha10-core message type - so it's equally usable for any other
//! single-axis stabilization loop (attitude, velocity, ...) a future behaviour node needs.
//!
//! There was no existing numeric PID *loop* in the repo to reuse: `mecha10_controllers::motor::
//! PidGains` and `mecha10_drivers_motor::common::PIDGains` are just `{kp, ki, kd}` config structs
//! handed to hardware/firmware that runs its own closed loop - neither crate implements the
//! actual error-integration math, and `behavior-runtime` (where this is used) doesn't depend on
//! either of those driver-layer crates. `PidGains` is kept as the field-naming precedent.

use serde::{Deserialize, Serialize};

/// Tunable gains for a [`PidController`]. Mirrors the field names/defaults of
/// `mecha10_controllers::motor::PidGains` for consistency, but is defined locally so this crate
/// doesn't take on a dependency on the driver-layer `controllers` crate just for three floats.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct PidGains {
    /// Proportional gain - reacts to the instantaneous error.
    pub kp: f64,
    /// Integral gain - reacts to the accumulated error over time (eliminates steady-state error).
    pub ki: f64,
    /// Derivative gain - reacts to the rate of change of the error (damps oscillation).
    pub kd: f64,
}

impl Default for PidGains {
    fn default() -> Self {
        Self {
            kp: 1.0,
            ki: 0.0,
            kd: 0.0,
        }
    }
}

/// A standard PID controller with output clamping and integral anti-windup.
///
/// Call [`PidController::update`] once per control tick with the current setpoint, measurement,
/// and elapsed time (`dt`, in seconds) since the previous call; it returns the control output.
#[derive(Debug, Clone)]
pub struct PidController {
    gains: PidGains,
    /// Accumulated error, clamped to `integral_limits` each update (anti-windup).
    integral: f64,
    /// Error from the previous `update` call, used for the derivative term. `None` until the
    /// first call, so the derivative term is skipped (treated as 0) on that first call instead
    /// of producing a spurious spike from an assumed-zero previous error.
    previous_error: Option<f64>,
    /// Clamp applied to the final output, as `(min, max)`.
    output_limits: (f64, f64),
    /// Clamp applied to the accumulated integral term, as `(min, max)` - bounds how much
    /// "wind-up" can accumulate while the output is saturated at `output_limits`.
    integral_limits: (f64, f64),
}

impl PidController {
    /// Create a new controller with the given gains and output clamp. Integral wind-up is
    /// clamped to the same range as the output by default - see [`Self::with_integral_limits`]
    /// to widen/narrow that independently.
    pub fn new(gains: PidGains, output_limits: (f64, f64)) -> Self {
        Self {
            gains,
            integral: 0.0,
            previous_error: None,
            output_limits,
            integral_limits: output_limits,
        }
    }

    /// Override the integral anti-windup clamp independently of the output clamp.
    pub fn with_integral_limits(mut self, integral_limits: (f64, f64)) -> Self {
        self.integral_limits = integral_limits;
        self
    }

    /// Advance the controller by one tick and return the (clamped) control output.
    ///
    /// `dt` is the elapsed time in seconds since the previous call. A non-positive `dt` skips
    /// the integral/derivative terms for this tick (treated as a proportional-only response)
    /// rather than dividing by zero or integrating backwards.
    pub fn update(&mut self, setpoint: f64, measurement: f64, dt: f64) -> f64 {
        let error = setpoint - measurement;

        let derivative = if dt > 0.0 {
            self.integral = (self.integral + error * dt).clamp(self.integral_limits.0, self.integral_limits.1);
            let derivative = self
                .previous_error
                .map(|previous| (error - previous) / dt)
                .unwrap_or(0.0);
            self.previous_error = Some(error);
            derivative
        } else {
            0.0
        };

        let output = self.gains.kp * error + self.gains.ki * self.integral + self.gains.kd * derivative;
        output.clamp(self.output_limits.0, self.output_limits.1)
    }

    /// Clear accumulated integral/derivative state (e.g. when a behaviour restarts), so a
    /// stale integral from a previous run doesn't bias the next one.
    pub fn reset(&mut self) {
        self.integral = 0.0;
        self.previous_error = None;
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn proportional_only_tracks_error() {
        let gains = PidGains {
            kp: 2.0,
            ki: 0.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        let output = pid.update(10.0, 4.0, 0.1);
        assert_eq!(output, 12.0); // 2.0 * (10.0 - 4.0)
    }

    #[test]
    fn integral_accumulates_error_over_time() {
        let gains = PidGains {
            kp: 0.0,
            ki: 1.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        // error = 1.0 each tick, dt = 1.0s -> integral grows by 1.0 each call
        let first = pid.update(1.0, 0.0, 1.0);
        let second = pid.update(1.0, 0.0, 1.0);

        assert_eq!(first, 1.0);
        assert_eq!(second, 2.0);
    }

    #[test]
    fn derivative_reacts_to_rate_of_change_and_is_zero_on_first_call() {
        let gains = PidGains {
            kp: 0.0,
            ki: 0.0,
            kd: 1.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        // First call: no previous error, derivative term must not spike.
        let first = pid.update(10.0, 0.0, 1.0);
        assert_eq!(first, 0.0);

        // error went from 10.0 -> 5.0 over 1s: derivative = (5.0 - 10.0) / 1.0 = -5.0
        let second = pid.update(10.0, 5.0, 1.0);
        assert_eq!(second, -5.0);
    }

    #[test]
    fn output_is_clamped_to_configured_limits() {
        let gains = PidGains {
            kp: 100.0,
            ki: 0.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-1.0, 1.0));

        assert_eq!(pid.update(10.0, 0.0, 1.0), 1.0);
        assert_eq!(pid.update(-10.0, 0.0, 1.0), -1.0);
    }

    #[test]
    fn integral_is_clamped_to_prevent_windup() {
        let gains = PidGains {
            kp: 0.0,
            ki: 1.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0)).with_integral_limits((-2.0, 2.0));

        // Each call accumulates error=5.0 * dt=1.0 = +5.0 to the integral, but it's clamped to 2.0.
        for _ in 0..5 {
            pid.update(5.0, 0.0, 1.0);
        }

        assert_eq!(pid.update(5.0, 0.0, 1.0), 2.0);
    }

    #[test]
    fn non_positive_dt_skips_integral_and_derivative_terms() {
        let gains = PidGains {
            kp: 1.0,
            ki: 1.0,
            kd: 1.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        pid.update(10.0, 0.0, 1.0); // seeds integral=10.0, previous_error=10.0
        let output = pid.update(10.0, 0.0, 0.0);

        // The integral/derivative terms aren't updated this tick (dt <= 0), but the previously
        // accumulated integral still contributes: kp * error + ki * integral + kd * 0
        // = 1.0 * 10.0 + 1.0 * 10.0 + 0.0 = 20.0
        assert_eq!(output, 20.0);
    }

    #[test]
    fn reset_clears_integral_and_derivative_state() {
        let gains = PidGains {
            kp: 0.0,
            ki: 1.0,
            kd: 1.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        pid.update(5.0, 0.0, 1.0);
        pid.reset();

        // With state cleared, the derivative term should again be 0.0 on the next call (no
        // previous error to diff against), matching the very first call's behavior.
        let output = pid.update(5.0, 0.0, 1.0);
        assert_eq!(output, 5.0); // ki * integral = 1.0 * 5.0, kd term is 0.0
    }

    #[test]
    fn converges_to_setpoint_over_repeated_ticks() {
        // A damped PI loop against a simple first-order plant (measurement moves toward the
        // last output each tick) should converge close to the setpoint, not just produce a
        // single sane-looking output.
        let gains = PidGains {
            kp: 0.6,
            ki: 0.3,
            kd: 0.05,
        };
        let mut pid = PidController::new(gains, (-10.0, 10.0));

        let mut measurement = 0.0_f64;
        let setpoint = 5.0;
        let dt = 0.1;

        for _ in 0..200 {
            let output = pid.update(setpoint, measurement, dt);
            measurement += output * dt;
        }

        assert!(
            (measurement - setpoint).abs() < 0.1,
            "expected convergence near {setpoint}, got {measurement}"
        );
    }
}