I have attached a rewrite of the lowpass component. This adds an option
to use either IIR or FIR filtering. It also adds optional integer inputs
and outputs (useful for MPGs) and scaling.
FIR filtering is better than IIR for smoothing noisy inputs and it also
works really well for smoothing MPGs. If you have a fairly low
resolution MPG you can have an issue where the individual pulses at low
speeds and make the machine shake. Feed the MPG through a lowpass with
scale=100 and time=150. The output goes to your axis jog-counts. Note
that the output in this case is 100x the input so you need to adjust
jog-scale to suit. You may need to play with the time a bit. Use as low
a value as you can get away with.
With the default settings this version is of course a drop-in
replacement for the previous version of this component and uses the same
algorithm.
Les
component lowpass "Low-pass filter";
notes """
This filter can be either implemented as a finite impulse response
low-pass filter (FIR) or an inifinite impulse response low pass filter
(IIR).
What is the difference? An IIR filter behaves much like an analog filter
with a logarithmic response to a step input. Think of two buckets
connected with with a pipe. If you quickly fill one bucket (step input)
the second bucket will start to fill quickly but as the levels get
closer the flow rate gets slower and slower. Ignoring real-world
considerations it would take an infinite time for the two levels
to match perfectly. The \\fBgain\\fR pin effectively defines the
diameter of the pipe. The smaller the value the slower the flow.
A FIR filter on the other hand always responds to a step input in a
fixed time. In the bucket analogy the flow rate remains constant until
the buckets match, then it stops. This mode can for example be used to
filter the output of an MPG. If you have an MPG scaled for fairly large
increments, on machines with high acceleration you can end up with the
machine vibrating when you move the MPG slowly as it makes a series of
step moves. A FIR filter could be added to spread the increments over a
longer time. This smooths out low speed resonse while not significantly
affecting high speed response (the delay of the filter is masked by the
machine's acceleration limits at high speed).
""";
option extra_setup yes;
option extra_cleanup yes;
include <limits.h>;
include <rtapi_slab.h>; //rtapi_kmalloc and friends
pin in float in "Floating point input";
pin in signed in_int = LONG_MIN """Integer input. Note: Only connect one
input. If you use both, \\fBin\\fR will be ignored.
""";
pin out float out "Floating point output";
pin out signed out_int """Integer output. This is the floating point
output rounded down to the nearest integer.
""";
pin in bit load """When TRUE, copy \\fBin\\fR to \\fBout\\fR ignoring
the filter equation.
""";
param rw unsigned time = 0 """Enable FIR filtering and set the time in
thread periods. This is implemented as a simple moving average
filter. If this pin is 0 an IIR filter will be used.
The response time of the filter will be:
\\fBtime\\fR * T
For a standard servo thread T is 0.001 seconds.
NOTES: The maximum time is 10000. If you specify a larger value it will
be limited to 10000. If you need very low frequency response, use
IIR filtering instead.
The filter is cleared to 0 when \\fBtime\\fR is changed.
""";
param rw float scale = 1 """Scale the input to the filter. The input
will be multipied by this value.
For example if using this to smooth an MPG you may want to use a scale
factor of 100 to effectively increase the resolution of the filter
""";
param rw float gain """IIR filter feedback gain.
The IIR filter implemented is equivalent to a unity-gain
continuous-time single-pole low-pass filter that is preceded
by a zero-order-hold and sampled at a fixed period. For a pole
at \\fB-a\\fR (radians/seconds) the corresponding continuous-time
lowpass filter LaPlace transfer function is:
\\fBH(s) = a/(s + a)\\fR
For a sampling period \\fBT\\fR (seconds), the gain for this
Hal lowpass component is:
\\fBgain = 1 - e^(-a * T)\\fR
e = 2.71828 https://en.wikipedia.org/wiki/E_(mathematical_constant)
\\fBExamples:\\fR
T = 0.001 seconds (typical servo thread period)
a = (2*pi*100) (\\fB100Hz\\fR bandwith single pole)
gain = \\fB0.466\\fR
T = 0.001 seconds (typical servo thread period)
a = (2*pi*10) ( \\fB10Hz\\fR bandwith single pole)
gain = \\fB0.0609\\fR
T = 0.001 seconds (typical servo thread period)
a = (2*pi*1) ( \\fB1Hz\\fR bandwith single pole)
gain = \\fB0.0063\\fR
NOTE: Ignored if \\fBtime\\fR is greater than 0
""";
variable unsigned prev_time = 0;
variable double *buff_ptr;
variable double acc = 0;
variable int index = 0;
function _;
license "GPL";
author "Les Newell";
;;
#define MAX_TIME 10000 //10k samples max
/*
pre-allocate memory for the filter.
If this was defined as a variable array it would be placed in HAL shared
memory which is quite small.
*/
EXTRA_SETUP() {
buff_ptr = rtapi_kmalloc(MAX_TIME * sizeof(double), RTAPI_GFP_KERNEL);
if(buff_ptr == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, "lowpass: Unable to allocate memory\n");
return(-1);
}
return(0);
}
FUNCTION(_) {
if(time != prev_time){
prev_time = time;
if(time > MAX_TIME) {
rtapi_print_msg(RTAPI_MSG_ERR, "lowpass: Time too long. Limiting to
%i samples\n", MAX_TIME);
time = MAX_TIME;
}
memset(buff_ptr, 0, time * sizeof(double));
index = 0;
acc = 0;
}
double input = in;
if(in_int != LONG_MIN) input = (double)in_int;
input *= scale;
if(load){ //disabled
out = input;
}else if(buff_ptr == NULL){ //IIR
out += (input - out) * gain;
}else{ //FIR
acc += input - buff_ptr[index];
buff_ptr[index] = input;
out = acc / time;
index++;
if(index >= time || index >= MAX_TIME) index = 0;
}
out_int = (int)out;
}
EXTRA_CLEANUP() {
struct __comp_state * __comp_inst = __comp_first_inst;
while(__comp_inst){
rtapi_kfree(buff_ptr);
__comp_inst = __comp_inst->_next;
}
}
_______________________________________________
Emc-developers mailing list
Emc-developers@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/emc-developers
