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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
|
/******************************************************************************
*
* 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 LinuxCNC 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";
author "Peter G. Vavaroutsos";
;;
#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;
}
}
|
