std_rs/device_support/
epid_fast.rs

1use std::sync::{Arc, Mutex};
2use std::time::Instant;
3
4use epics_base_rs::error::CaResult;
5use epics_base_rs::server::device_support::{DeviceReadOutcome, DeviceSupport};
6use epics_base_rs::server::record::Record;
7
8use crate::records::epid::EpidRecord;
9
10/// Fast Epid device support using asyn driver for high-speed (1+ kHz) PID.
11///
12/// Ported from `devEpidFast.c`. The PID computation runs in a background
13/// tokio task driven by asyn interrupt callbacks, not during record
14/// processing. The record merely copies parameters to/from the fast
15/// computation thread.
16///
17/// # Architecture
18///
19/// ```text
20/// ┌─────────────┐    interrupt     ┌──────────────────┐
21/// │ asyn driver  │ ──────────────► │ PID callback task │
22/// │ (input ADC)  │    (new cval)   │  (tokio::spawn)   │
23/// └─────────────┘                  │  runs do_pid()    │
24///                                  │  writes output    │
25///       ┌──────────────────────────┤  to output driver │
26///       │  shared EpidFastPvt      └──────────────────┘
27///       │  (Arc<Mutex>)                    ▲
28///       ▼                                  │
29/// ┌─────────────┐  read()         params   │
30/// │ EpidRecord   │ ◄─────── copy ──────────┘
31/// │ (process)    │ ────────► copy ──────────►
32/// └─────────────┘  results
33/// ```
34///
35/// The `start_callback_loop()` method spawns the background task.
36/// Call it after connecting to the asyn input port.
37pub struct EpidFastDeviceSupport {
38    pvt: Arc<Mutex<EpidFastPvt>>,
39}
40
41/// Private state for the fast PID loop, shared between the
42/// record process thread and the interrupt callback task.
43pub struct EpidFastPvt {
44    // PID parameters (copied from record on each process cycle)
45    pub kp: f64,
46    pub ki: f64,
47    pub kd: f64,
48    pub drvh: f64,
49    pub drvl: f64,
50    pub val: f64, // setpoint
51    pub fbon: bool,
52    pub fmod: i16,
53
54    // PID state (updated by callback, read by record process)
55    pub cval: f64,
56    pub oval: f64,
57    pub err: f64,
58    pub p: f64,
59    pub i: f64,
60    pub d: f64,
61    pub dt: f64,
62    pub ct: Instant,
63    pub fbop: bool,
64
65    // Averaging
66    pub num_average: u32,
67    pub accumulated: f64,
68    pub count: u32,
69
70    // Output port writer (set by start_callback_loop)
71    pub output_writer: Option<Arc<Mutex<dyn FnMut(f64) + Send>>>,
72}
73
74impl Default for EpidFastPvt {
75    fn default() -> Self {
76        let now = Instant::now();
77        Self {
78            kp: 0.0,
79            ki: 0.0,
80            kd: 0.0,
81            drvh: 0.0,
82            drvl: 0.0,
83            val: 0.0,
84            fbon: false,
85            fmod: 0,
86            cval: 0.0,
87            oval: 0.0,
88            err: 0.0,
89            p: 0.0,
90            i: 0.0,
91            d: 0.0,
92            dt: 0.0,
93            ct: now,
94            fbop: false,
95            num_average: 1,
96            accumulated: 0.0,
97            count: 0,
98            output_writer: None,
99        }
100    }
101}
102
103impl EpidFastPvt {
104    /// Execute one PID cycle on new data. Called from the interrupt callback task.
105    /// After computing the output, writes to the output port if configured.
106    pub fn do_pid(&mut self, new_cval: f64) {
107        // Averaging
108        self.accumulated += new_cval;
109        self.count += 1;
110        if self.count < self.num_average {
111            return;
112        }
113        let cval = self.accumulated / self.count as f64;
114        self.accumulated = 0.0;
115        self.count = 0;
116
117        let pcval = self.cval;
118        self.cval = cval;
119
120        let ct = Instant::now();
121        let dt = ct.duration_since(self.ct).as_secs_f64();
122        self.ct = ct;
123        self.dt = dt;
124
125        let ep = self.err;
126        let mut oval = self.oval;
127
128        match self.fmod {
129            0 => {
130                // PID mode
131                let e = self.val - cval;
132                let de = e - ep;
133                self.p = self.kp * e;
134                let di = self.kp * self.ki * e * dt;
135
136                if self.fbon {
137                    if !self.fbop {
138                        self.i = self.oval;
139                    } else {
140                        if (oval > self.drvl && oval < self.drvh)
141                            || (oval >= self.drvh && di < 0.0)
142                            || (oval <= self.drvl && di > 0.0)
143                        {
144                            self.i += di;
145                            self.i = self.i.clamp(self.drvl, self.drvh);
146                        }
147                    }
148                }
149                if self.ki == 0.0 {
150                    self.i = 0.0;
151                }
152                self.d = if dt > 0.0 {
153                    self.kp * self.kd * (de / dt)
154                } else {
155                    0.0
156                };
157                self.err = e;
158                oval = self.p + self.i + self.d;
159            }
160            1 => {
161                // MaxMin mode
162                if self.fbon {
163                    if !self.fbop {
164                        oval = self.oval;
165                    } else {
166                        let e = cval - pcval;
167                        let sign = if self.d > 0.0 { 1.0 } else { -1.0 };
168                        let sign = if (self.kp > 0.0 && e < 0.0) || (self.kp < 0.0 && e > 0.0) {
169                            -sign
170                        } else {
171                            sign
172                        };
173                        self.d = self.kp * sign;
174                        oval = self.oval + self.d;
175                    }
176                }
177            }
178            _ => {}
179        }
180
181        // Clamp output
182        oval = oval.clamp(self.drvl, self.drvh);
183        self.oval = oval;
184        self.fbop = self.fbon;
185
186        // Write output to hardware if configured
187        if self.fbon {
188            if let Some(ref writer) = self.output_writer {
189                if let Ok(mut w) = writer.lock() {
190                    w(self.oval);
191                }
192            }
193        }
194    }
195}
196
197impl Default for EpidFastDeviceSupport {
198    fn default() -> Self {
199        Self::new()
200    }
201}
202
203impl EpidFastDeviceSupport {
204    pub fn new() -> Self {
205        Self {
206            pvt: Arc::new(Mutex::new(EpidFastPvt::default())),
207        }
208    }
209
210    /// Get a handle to the shared PID state for callback registration.
211    pub fn pvt(&self) -> Arc<Mutex<EpidFastPvt>> {
212        Arc::clone(&self.pvt)
213    }
214
215    /// Start the interrupt-driven PID callback loop.
216    ///
217    /// Spawns a tokio task that receives new readback values from `input_rx`
218    /// and runs `do_pid()` on each. This is the high-speed PID path that
219    /// runs at the interrupt rate (1kHz+), independent of record processing.
220    ///
221    /// `input_rx`: receives new controlled-variable values from the input driver
222    /// `output_fn`: called with each new output value (writes to output driver)
223    pub fn start_callback_loop(
224        &self,
225        mut input_rx: tokio::sync::mpsc::Receiver<f64>,
226        output_fn: Arc<Mutex<dyn FnMut(f64) + Send>>,
227    ) {
228        let pvt = Arc::clone(&self.pvt);
229
230        // Store the output writer in pvt
231        {
232            let mut p = pvt.lock().unwrap();
233            p.output_writer = Some(output_fn);
234        }
235
236        tokio::spawn(async move {
237            while let Some(new_cval) = input_rx.recv().await {
238                let mut p = pvt.lock().unwrap();
239                p.do_pid(new_cval);
240            }
241        });
242    }
243
244    /// Start from an asyn interrupt subscription.
245    ///
246    /// Subscribes to Float64 interrupts from the given broadcast sender
247    /// and feeds them into the PID callback loop.
248    pub fn start_from_asyn_interrupts(
249        &self,
250        mut interrupt_rx: tokio::sync::broadcast::Receiver<asyn_rs::interrupt::InterruptValue>,
251        input_reason: usize,
252        output_fn: Arc<Mutex<dyn FnMut(f64) + Send>>,
253    ) {
254        let pvt = Arc::clone(&self.pvt);
255
256        {
257            let mut p = pvt.lock().unwrap();
258            p.output_writer = Some(output_fn);
259        }
260
261        tokio::spawn(async move {
262            loop {
263                match interrupt_rx.recv().await {
264                    Ok(iv) => {
265                        if iv.reason == input_reason {
266                            let v = match &iv.value {
267                                asyn_rs::param::ParamValue::Float64(f) => Some(*f),
268                                asyn_rs::param::ParamValue::Int32(i) => Some(*i as f64),
269                                asyn_rs::param::ParamValue::Int64(i) => Some(*i as f64),
270                                _ => None,
271                            };
272                            if let Some(v) = v {
273                                let mut p = pvt.lock().unwrap();
274                                p.do_pid(v);
275                            }
276                        }
277                    }
278                    Err(tokio::sync::broadcast::error::RecvError::Closed) => break,
279                    Err(tokio::sync::broadcast::error::RecvError::Lagged(_)) => {
280                        // Dropped some interrupts — continue
281                    }
282                }
283            }
284        });
285    }
286
287    /// Copy parameters from record to fast PID state.
288    fn update_params_from_record(&self, epid: &EpidRecord) {
289        let mut pvt = self.pvt.lock().unwrap();
290        pvt.kp = epid.kp;
291        pvt.ki = epid.ki;
292        pvt.kd = epid.kd;
293        pvt.drvh = epid.drvh;
294        pvt.drvl = epid.drvl;
295        pvt.val = epid.val;
296        pvt.fbon = epid.fbon != 0;
297        pvt.fmod = epid.fmod;
298    }
299
300    /// Copy computed results from fast PID state back to record.
301    fn update_record_from_params(&self, epid: &mut EpidRecord) {
302        let pvt = self.pvt.lock().unwrap();
303        epid.cval = pvt.cval;
304        epid.oval = pvt.oval;
305        epid.err = pvt.err;
306        epid.p = pvt.p;
307        epid.i = pvt.i;
308        epid.d = pvt.d;
309        epid.dt = pvt.dt;
310        epid.fbop = if pvt.fbop { 1 } else { 0 };
311    }
312}
313
314impl DeviceSupport for EpidFastDeviceSupport {
315    fn dtyp(&self) -> &str {
316        "Fast Epid"
317    }
318
319    fn read(&mut self, record: &mut dyn Record) -> CaResult<DeviceReadOutcome> {
320        let epid = record
321            .as_any_mut()
322            .and_then(|a| a.downcast_mut::<EpidRecord>())
323            .expect("EpidFastDeviceSupport requires an EpidRecord");
324
325        // Copy parameters to fast PID (so callback loop uses latest gains)
326        self.update_params_from_record(epid);
327        // Copy latest results back to record (for display/alarm)
328        self.update_record_from_params(epid);
329        Ok(DeviceReadOutcome::computed())
330    }
331
332    fn write(&mut self, _record: &mut dyn Record) -> CaResult<()> {
333        Ok(())
334    }
335}
std_rs/device_support/epid_fast.rs

std_rs/device_support/
epid_fast.rs
