src/hal/components/limit2.comp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

component limit2 "Limit the output signal to fall between min and max and limit its slew rate to less than maxv per second.  When the signal is a position, this means that position and velocity are limited.";
pin in float in;
pin out float out;
pin in bit load "When TRUE, immediately set \\fBout\\fB to \\fBin\\fR, ignoring maxv";
param rw float min_=-1e20;
param rw float max_=1e20;
param rw float maxv=1e20;
option data limit2_data;
function _;
license "GPL";
;;

typedef struct { double old_out; } limit2_data;

#ifndef clamp
static inline double clamp(double v, double sub, double sup) {
    if(v < sub) return sub;
    if(v > sup) return sup;
    return v;
}
#endif

FUNCTION(_) {
    double tmp = in;
    double maxdelta = maxv * fperiod;
    tmp = clamp(tmp, min_, max_);
    if(load) { out = data.old_out = tmp; return; }
    tmp = clamp(tmp, data.old_out - maxdelta, data.old_out + maxdelta);
    data.old_out = tmp;
    out = tmp;
}