/******************************************************************************
*
* Copyright (C) 2007 Peter G. Vavaroutsos <pete AT vavaroutsos DOT com>
*
*
* This module implements a biquad IIR filter.
*
******************************************************************************
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of version 2 of the GNU General
* Public License as published by the Free Software Foundation.
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
* THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR
* ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE
* TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of
* harming persons must have provisions for completely removing power
* from all motors, etc, before persons enter any danger area. All
* machinery must be designed to comply with local and national safety
* codes, and the authors of this software can not, and do not, take
* any responsibility for such compliance.
*
* This code was written as part of the EMC HAL project. For more
* information, go to www.linuxcnc.org.
*
******************************************************************************/
component biquad "Biquad IIR filter";
description """Biquad IIR filter. Implements the following transfer function:
H(z) = (n0 + n1z-1 + n2z-2) / (1+ d1z-1 + d2z-2)""";
pin in float in "Filter input.";
pin out float out "Filter output.";
pin in bit enable = 0 "Filter enable. When false, the \\fBin\\fR pin \
is passed to the \\fBout\\fR pin without any filtering. \
A \\fBtransition from false to true\\fR causes filter \
coefficients to be calculated according to the current \
\\fBtype\\fR and the describing pin and parameter settings";
pin out bit valid = 0 "When false, indicates an error occurred when calculating \
filter coefficients (require 2>\\fBQ\\fR>0.5 and \\fBf0\\fR>sampleRate/2)";
pin in u32 type_ = 0 "Filter type determines the type of filter \
coefficients calculated. When 0, coefficients must be loaded directly \
from the \\fBn0,n1,n2,d1\\fR params. When 1, \
a low pass filter is created specified by the \\fBf0,Q\\fR pins. \
When 2, a notch filter is created specified by the \\fBf0,Q\\fR pins.";
pin in float f0 = 250.0 "The corner frequency of the filter.";
pin in float Q = 0.7071 "The Q of the filter.";
param rw float d1 = 0.0 "1st-delayed denominator coef";
param rw float d2 = 0.0 "2nd-delayed denominator coef";
param rw float n0 = 1.0 "non-delayed numerator coef";
param rw float n1 = 0.0 "1st-delayed numerator coef";
param rw float n2 = 0.0 "2nd-delayed numerator coef";
pin out float s1 = 0.0 "1st-delayed internal state (for debug only)";
pin out float s2 = 0.0 "2nd-delayed internal state (for debug only)";
option data Internal;
option extra_setup;
function _;
license "GPL";
;;
#include <float.h>
#include <rtapi_math.h>
#define PI 3.141592653589
void Biquad_CalcCoeffs(unsigned long period);
typedef enum {
TYPE_DIRECT,
TYPE_LOW_PASS,
TYPE_NOTCH,
} Type;
typedef struct {
hal_bit_t lastEnable;
} Internal;
EXTRA_SETUP()
{
data.lastEnable = 0;
return(0);
}
FUNCTION(_)
{
if(data.lastEnable != enable){
data.lastEnable = enable;
if(enable) do {
double sampleRate, w0, alpha;
double b0, b1, b2, a0, a1, a2;
// If not direct coefficient loading.
if(type_ != TYPE_DIRECT){
valid = 0;
sampleRate = 1.0 / (period * 1e-9);
if((f0 > sampleRate / 2.0) || (Q > 2.0) || (Q < 0.5))
break;
w0 = 2.0 * PI * f0 / sampleRate;
alpha = sin(w0) / (2.0 * Q);
if(type_ == TYPE_LOW_PASS){
b0 = (1.0 - cos(w0)) / 2.0;
b1 = 1.0 - cos(w0);
b2 = (1.0 - cos(w0)) / 2.0;
a0 = 1.0 + alpha;
a1 = -2.0 * cos(w0);
a2 = 1.0 - alpha;
}else if(type_ == TYPE_NOTCH){
b0 = 1.0;
b1 = -2.0 * cos(w0);
b2 = 1.0;
a0 = 1.0 + alpha;
a1 = -2.0 * cos(w0);
a2 = 1.0 - alpha;
}else{
break;
}
n0 = b0 / a0;
n1 = b1 / a0;
n2 = b2 / a0;
d1 = a1 / a0;
d2 = a2 / a0;
s1 = s2 = 0.0;
}
valid = 1;
} while(0);
}
if(!enable || !valid){
out = in;
}else{
// Transposed direct form II.
out = in * n0 + s1;
s1 = in * n1 - out * d1 + s2;
s2 = in * n2 - out * d2;
}
}