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
|
/******************************************************************************
*
* 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., 59 Temple Place, Suite 330, Boston, MA 02111 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 in is passed to out \
without any filtering. A transition from false to true causes filter \
coefficients to be calculated according to parameters";
pin out bit valid = 0 "When false, indicates an error occured when caclulating \
filter coefficients.";
param rw u32 type_ = 0 "Filter type determines the type of filter \
coefficients calculated. When 0, coefficients must be loaded directly. When 1, \
a low pass filter is created. When 2, a notch filter is created.";
param rw float f0 = 250.0 "The corner frequency of the filter.";
param rw 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";
param rw float s1 = 0.0;
param rw float s2 = 0.0;
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;
}
}
|
