witness_controller/
lib.rs

1// SPDX-License-Identifier: AGPL-3.0-or-later
2// Commercial license available
3// © Concepts 1996–2026 Miroslav Šotek. All rights reserved.
4// © Code 2020–2026 Miroslav Šotek. All rights reserved.
5// ORCID: 0009-0009-3560-0851
6// Contact: www.anulum.li | protoscience@anulum.li
7// Loop Timing Witness — controllers/rust/src/lib.rs
8
9//! Bit-exact Q8.24 controllers. Coefficients are held stable until reset.
10//! Products accumulate in i128; division floors negative fractional values.
11//! These kernels do not establish physical timing or controller stability.
12//! The library uses only core arithmetic and requires neither an allocator nor an OS.
13
14#![no_std]
15
16const SCALE: i128 = 1 << 24;
17
18/// C-layout signed Q8.24 coefficients and output/integral bounds.
19#[derive(Clone, Copy, Debug, PartialEq, Eq)]
20#[repr(C)]
21pub struct Coefficients {
22    /// Proportional error gain, nonnegative.
23    pub kp: i32,
24    /// Integral gain times sample period, nonnegative.
25    pub ki_period: i32,
26    /// Filter decay in the inclusive interval [0, 2^24].
27    pub derivative_decay: i32,
28    /// Filtered derivative gain on the measurement difference, nonnegative.
29    pub derivative_gain: i32,
30    /// Discrete LQR position state feedback.
31    pub position_gain: i32,
32    /// Discrete LQR velocity state feedback; zero for the scalar thermal model.
33    pub velocity_gain: i32,
34    /// Steady-state reference prefilter gain.
35    pub reference_gain: i32,
36    /// Minimum actuator command.
37    pub output_min: i32,
38    /// Maximum actuator command.
39    pub output_max: i32,
40    /// Minimum PID integral state.
41    pub integral_min: i32,
42    /// Maximum PID integral state.
43    pub integral_max: i32,
44}
45
46/// Refused coefficient range; kernels return before changing any state.
47#[derive(Clone, Copy, Debug, PartialEq, Eq)]
48pub struct InvalidCoefficients;
49
50impl Coefficients {
51    /// Check bounds, nonnegative PID gains and stable derivative filter decay.
52    pub fn validate(&self) -> Result<(), InvalidCoefficients> {
53        if self.output_min > self.output_max
54            || self.integral_min > self.integral_max
55            || self.output_min > 0
56            || self.output_max < 0
57            || self.integral_min > 0
58            || self.integral_max < 0
59            || self.kp < 0
60            || self.ki_period < 0
61            || self.derivative_gain < 0
62            || self.derivative_decay < 0
63            || i128::from(self.derivative_decay) > SCALE
64        {
65            return Err(InvalidCoefficients);
66        }
67        Ok(())
68    }
69}
70
71/// C-layout cycle-tagged command plus the observable PID state and clipping decisions.
72#[derive(Clone, Copy, Debug, PartialEq, Eq)]
73#[repr(C)]
74pub struct Command {
75    /// Original input cycle, unchanged by arithmetic latency.
76    pub cycle: u32,
77    /// Saturated actuator command.
78    pub command: i32,
79    /// PID integral state after this sample; zero for LQR.
80    pub integral: i32,
81    /// PID derivative state after this sample; zero for LQR.
82    pub derivative: i32,
83    /// Final unsaturated actuator value exceeded a configured limit.
84    pub clipped: bool,
85    /// Conditional integration refused a step that would worsen saturation.
86    pub integral_held: bool,
87}
88
89/// C-layout PID memory, with reset marking the next measurement as the derivative origin.
90#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
91#[repr(C)]
92pub struct PidState {
93    /// Current bounded integral.
94    pub integral: i32,
95    /// Current bounded filtered derivative on measurement.
96    pub derivative: i32,
97    previous_position: i32,
98    initialized: bool,
99}
100
101/// Saturate a complete wide result into the validated signed raw interval.
102fn clamp(value: i128, lower: i32, upper: i32) -> i32 {
103    value.clamp(i128::from(lower), i128::from(upper)) as i32
104}
105
106impl PidState {
107    /// Clear all memory and establish a zero-derivative first sample.
108    pub fn reset(&mut self) {
109        *self = Self::default();
110    }
111
112    /// Update a conditional-integration PID with derivative on measurement.
113    ///
114    /// The prior integral is used to detect output saturation. If saturation
115    /// and error have the same direction, retain that integral; otherwise accept
116    /// the proposed integral before computing the command. First-sample derivative
117    /// is zero. Invalid coefficients leave state unchanged. Gains remain stable
118    /// until reset.
119    pub fn step(
120        &mut self,
121        coefficients: &Coefficients,
122        cycle: u32,
123        reference: i32,
124        position: i32,
125    ) -> Result<Command, InvalidCoefficients> {
126        coefficients.validate()?;
127        let error = i128::from(reference) - i128::from(position);
128        let difference = i128::from(position) - i128::from(self.previous_position);
129        let derivative = if self.initialized {
130            clamp(
131                (i128::from(coefficients.derivative_decay) * i128::from(self.derivative)
132                    - i128::from(coefficients.derivative_gain) * difference)
133                    .div_euclid(SCALE),
134                i32::MIN,
135                i32::MAX,
136            )
137        } else {
138            0
139        };
140        let proposed_integral = clamp(
141            (i128::from(self.integral) * SCALE + i128::from(coefficients.ki_period) * error)
142                .div_euclid(SCALE),
143            coefficients.integral_min,
144            coefficients.integral_max,
145        );
146        let proportional = i128::from(coefficients.kp) * error;
147        let output = |integral| {
148            (proportional + i128::from(integral) * SCALE + i128::from(derivative) * SCALE)
149                .div_euclid(SCALE)
150        };
151        let provisional = output(self.integral);
152        let held = (provisional >= i128::from(coefficients.output_max) && error > 0)
153            || (provisional <= i128::from(coefficients.output_min) && error < 0);
154        let integral = if held {
155            self.integral
156        } else {
157            proposed_integral
158        };
159        let raw = output(integral);
160        self.integral = integral;
161        self.derivative = derivative;
162        self.previous_position = position;
163        self.initialized = true;
164        Ok(Command {
165            cycle,
166            command: clamp(raw, coefficients.output_min, coefficients.output_max),
167            integral,
168            derivative,
169            clipped: raw < i128::from(coefficients.output_min)
170                || raw > i128::from(coefficients.output_max),
171            integral_held: held,
172        })
173    }
174}
175
176/// Apply full-state discrete LQR and a reference prefilter, then saturate output.
177///
178/// The kernel computes floor((nr*r-kx*y-kv*v)/2^24). Supplied gains need an
179/// independently recorded Riccati design and quantized closed-loop stability
180/// check. Invalid coefficients do not yield a command.
181pub fn lqr_step(
182    coefficients: &Coefficients,
183    cycle: u32,
184    reference: i32,
185    position: i32,
186    velocity: i32,
187) -> Result<Command, InvalidCoefficients> {
188    coefficients.validate()?;
189    let raw = (i128::from(coefficients.reference_gain) * i128::from(reference)
190        - i128::from(coefficients.position_gain) * i128::from(position)
191        - i128::from(coefficients.velocity_gain) * i128::from(velocity))
192    .div_euclid(SCALE);
193    Ok(Command {
194        cycle,
195        command: clamp(raw, coefficients.output_min, coefficients.output_max),
196        integral: 0,
197        derivative: 0,
198        clipped: raw < i128::from(coefficients.output_min)
199            || raw > i128::from(coefficients.output_max),
200        integral_held: false,
201    })
202}
witness_controller/lib.rs

witness_controller/
lib.rs
