std_rs/device_support/
epid_soft.rs

1use std::time::Instant;
2
3use epics_base_rs::error::CaResult;
4use epics_base_rs::server::device_support::{DeviceReadOutcome, DeviceSupport};
5use epics_base_rs::server::record::Record;
6
7use crate::records::epid::EpidRecord;
8
9/// Soft Channel device support for the epid record.
10///
11/// Implements the PID and MaxMin feedback algorithms.
12/// Ported from `devEpidSoft.c`.
13///
14/// PID algorithm:
15/// ```text
16/// E(n) = Setpoint - ControlledValue
17/// P(n) = KP * E(n)
18/// I(n) = I(n-1) + KP * KI * E(n) * dT  (with anti-windup)
19/// D(n) = KP * KD * (E(n) - E(n-1)) / dT
20/// Output = P + I + D
21/// ```
22pub struct EpidSoftDeviceSupport;
23
24impl Default for EpidSoftDeviceSupport {
25    fn default() -> Self {
26        Self::new()
27    }
28}
29
30impl EpidSoftDeviceSupport {
31    pub fn new() -> Self {
32        Self
33    }
34
35    /// Execute the PID algorithm on the epid record.
36    /// This is the core computation, equivalent to `do_pid()` in devEpidSoft.c.
37    pub fn do_pid(epid: &mut EpidRecord) {
38        let pcval = epid.cval;
39        let setp = epid.val;
40        let cval = epid.cval;
41
42        // Compute delta time
43        let ctp = epid.ct;
44        let ct = Instant::now();
45        let dt = ct.duration_since(ctp).as_secs_f64();
46
47        // Skip if delta time is less than minimum
48        if dt < epid.mdt {
49            return;
50        }
51
52        let kp = epid.kp;
53        let ki = epid.ki;
54        let kd = epid.kd;
55        let ep = epid.err;
56        let mut oval = epid.oval;
57        let mut p = epid.p;
58        let mut i = epid.i;
59        let mut d = epid.d;
60        let mut e = 0.0;
61
62        match epid.fmod {
63            0 => {
64                // PID mode
65                e = setp - cval;
66                let de = e - ep;
67                p = kp * e;
68
69                // Integral term with sanity checks
70                let di = kp * ki * e * dt;
71                if epid.fbon != 0 {
72                    if epid.fbop == 0 {
73                        // Feedback just transitioned OFF -> ON (bumpless turn-on).
74                        // Set integral term to current output value.
75                        // In the C code this reads from OUTL link; here we use
76                        // the current OVAL as the best available approximation.
77                        i = epid.oval;
78                    } else {
79                        // Anti-windup: only accumulate integral if output not saturated,
80                        // or if the integral change would move away from saturation.
81                        if (oval > epid.drvl && oval < epid.drvh)
82                            || (oval >= epid.drvh && di < 0.0)
83                            || (oval <= epid.drvl && di > 0.0)
84                        {
85                            i += di;
86                            if i < epid.drvl {
87                                i = epid.drvl;
88                            }
89                            if i > epid.drvh {
90                                i = epid.drvh;
91                            }
92                        }
93                    }
94                }
95                // If KI is zero, zero the integral term
96                if ki == 0.0 {
97                    i = 0.0;
98                }
99                // Derivative term
100                d = if dt > 0.0 { kp * kd * (de / dt) } else { 0.0 };
101                oval = p + i + d;
102            }
103            1 => {
104                // MaxMin mode
105                if epid.fbon != 0 {
106                    if epid.fbop == 0 {
107                        // Feedback just transitioned OFF -> ON.
108                        // Set output to current value (bumpless).
109                        oval = epid.oval;
110                    } else {
111                        e = cval - pcval;
112                        let sign = if d > 0.0 { 1.0 } else { -1.0 };
113                        let sign = if (kp > 0.0 && e < 0.0) || (kp < 0.0 && e > 0.0) {
114                            -sign
115                        } else {
116                            sign
117                        };
118                        d = kp * sign;
119                        oval = epid.oval + d;
120                    }
121                }
122            }
123            _ => {
124                tracing::warn!("Invalid feedback mode {} in epid record", epid.fmod);
125            }
126        }
127
128        // Clamp output to drive limits
129        if oval > epid.drvh {
130            oval = epid.drvh;
131        }
132        if oval < epid.drvl {
133            oval = epid.drvl;
134        }
135
136        // Update record fields
137        epid.ct = ct;
138        epid.dt = dt;
139        epid.err = e;
140        epid.cval = cval;
141
142        // Apply output deadband
143        if epid.odel == 0.0 || (epid.oval - oval).abs() > epid.odel {
144            epid.oval = oval;
145        }
146
147        epid.p = p;
148        epid.i = i;
149        epid.d = d;
150        epid.fbop = epid.fbon;
151    }
152}
153
154impl DeviceSupport for EpidSoftDeviceSupport {
155    fn dtyp(&self) -> &str {
156        "Epid Soft"
157    }
158
159    fn read(&mut self, record: &mut dyn Record) -> CaResult<DeviceReadOutcome> {
160        let epid = record
161            .as_any_mut()
162            .and_then(|a| a.downcast_mut::<EpidRecord>())
163            .expect("EpidSoftDeviceSupport requires an EpidRecord");
164
165        Self::do_pid(epid);
166        Ok(DeviceReadOutcome::computed())
167    }
168
169    fn write(&mut self, _record: &mut dyn Record) -> CaResult<()> {
170        Ok(())
171    }
172}
std_rs/device_support/epid_soft.rs

std_rs/device_support/
epid_soft.rs
