-
Notifications
You must be signed in to change notification settings - Fork 0
/
mylincurvev2.icomp
70 lines (53 loc) · 1.82 KB
/
mylincurvev2.icomp
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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
component mylincurvev2 "one-dimensional lookup table";
description
"""Performs a 1-dimensional lookup and interpolation. The x-val
parameters must be monotonic, though identical adjacent values are allowed.
(for example 0,0,0,10) for a 4-element curve.
For input values less than the x-val-00 breakpoint the y-val-00 is returned.
For x greater than the largest x-val-NN the yval corresponding to x-max is
returned (ie, no extrapolation is performed.)
Sample usage: loadrt lincurve-inst
newinst lincurve newone pincount=4 iprefix=lincurve4
for a 4-element curve.
The axis breakpoints should be set in the lincurve.x-val-NN parameters using
"setp", as should the corresponding y values.
""";
pin_ptr io float x-val-##[pincount] "axis breakpoints";
pin_ptr io float y-val-##[pincount] "output values to be interpolated";
pin_ptr in float in_ "The input value";
pin_ptr out float out_ "The output value";
pin_ptr io float out-io "The output value, compatible with PID gains";
variable hal_u32_t i = 0;
option extra_inst_setup 1;
instanceparam int pincount = 4;
option MAXCOUNT 16;
author "Andy Pugh";
license "GPL";
function _;
;;
FUNCTION(_)
{
hal_float_t f, x;
x = gf(in_);
if (x >= gf(x_val(local_pincount-1))) {
sf(out_, gf(y_val(local_pincount-1)));
sf(out_io, gf(out_));
return 0;
}
if (x <= gf(x_val(0))) {
sf(out_, gf(y_val(0)));
sf(out_io, gf(out_));
return 0;
}
while (x > gf(x_val(i+1))) { i++;}
while (x < gf((x_val(i)))) { i--;}
f = (x - gf(x_val(i))) / ( gf(x_val(i+1)) - gf(x_val(i)) ) ;
sf(out_, gf(y_val(i)) + f * (gf(y_val(i+1)) - gf(y_val(i)) )) ;
sf(out_io, gf(out_));
return 0;
}
EXTRA_INST_SETUP(){
// if (local_pincount > 16) local_pincount = 16;
if (local_pincount < 2) local_pincount = 2;
return 0;
}