-
Notifications
You must be signed in to change notification settings - Fork 32
/
Copy pathAudioFilterEqualizer_F32.cpp
192 lines (174 loc) · 8.19 KB
/
AudioFilterEqualizer_F32.cpp
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
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
/* AudioFilterEqualizer_F32.cpp
*
* Bob Larkin, W7PUA 8 May 2020
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "AudioFilterEqualizer_F32.h"
void AudioFilterEqualizer_F32::update(void) {
audio_block_f32_t *block, *block_new;
#if TEST_TIME_EQ
if (iitt++ >1000000) iitt = -10;
uint32_t t1, t2;
t1 = tElapse;
#endif
block = AudioStream_F32::receiveReadOnly_f32();
if (!block) return;
// If there's no coefficient table, give up.
if (cf32f == NULL) {
AudioStream_F32::release(block);
return;
}
block_new = AudioStream_F32::allocate_f32(); // get a block for the FIR output
if (block_new) {
//apply the FIR
arm_fir_f32(&fir_inst, block->data, block_new->data, block->length);
AudioStream_F32::transmit(block_new); // send the FIR output
AudioStream_F32::release(block_new);
}
AudioStream_F32::release(block);
#if TEST_TIME_EQ
t2 = tElapse;
if(iitt++ < 0) {Serial.print("At AnalyzePhase end, microseconds = "); Serial.println (t2 - t1); }
t1 = tElapse;
#endif
}
/* equalizerNew() calculates the Equalizer FIR filter coefficients. Works from:
* uint16_t equalizerNew(uint16_t _nBands, float32_t *feq, float32_t *adb,
uint16_t _nFIR, float32_t *_cf32f, float32_t kdb)
* nBands Number of equalizer bands
* feq Pointer to array feq[] of nBands breakpoint frequencies, fractions of sample rate, Hz
* adb Pointer to array aeq[] of nBands levels, in dB, for the feq[] defined frequency bands
* nFIR The number of FIR coefficients (taps) used in the equalzer
* cf32f Pointer to an array of float to hold FIR coefficients
* kdb A parameter that trades off sidelobe levels for sharpness of band transition.
* kdb=30 sharp cutoff, poor sidelobes
* kdb=60 slow cutoff, low sidelobes
*
* The arrays, feq[], aeq[] and cf32f[] are supplied by the calling .INO
*
* Returns: 0 if successful, or an error code if not.
* Errors: 1 = Too many bands, 50 max
* 2 = sidelobe level out of range, must be > 0
* 3 = nFIR out of range
*
* Note - This function runs at setup time, and there is no need to fret about
* processor speed. Likewise, local arrays are created on the stack and are
* available for other use when this function closes.
*/
uint16_t AudioFilterEqualizer_F32::equalizerNew(uint16_t _nBands, float32_t *feq, float32_t *adb,
uint16_t _nFIR, float32_t *_cf32f, float32_t kdb) {
uint16_t i, j;
uint16_t nHalfFIR;
float32_t beta, kbes;
float32_t q, xj2, scaleXj2, WindowWt;
float32_t fNorm[50]; // Normalized to the sampling frequency
float32_t aVolts[50]; // Convert from dB to "quasi-Volts"
mathDSP_F32 mathEqualizer; // For Bessel function
// Make private copies
cf32f = _cf32f;
nFIR = _nFIR;
nBands = _nBands;
// Check range of nFIR
if (nFIR<5 || nFIR>EQUALIZER_MAX_COEFFS)
return ERR_EQ_NFIR;
// The number of FIR coefficients needs to be odd
if (2*(nFIR/2) == nFIR)
nFIR -= 1; // We just won't use the last element of the array
nHalfFIR = (nFIR - 1)/2; // If nFIR=199, nHalfFIR=99
for (int kk = 0; kk<nFIR; kk++) // To be sure, zero the coefficients
cf32f[kk] = 0.0f;
// Convert dB to Voltage ratios, frequencies to fractions of sampling freq
if(nBands <2 || nBands>50) return ERR_EQ_BANDS;
for (i=0; i<nBands; i++) {
aVolts[i]=powf(10.0, (0.05*adb[i]));
fNorm[i]=feq[i]/sample_rate_Hz;
}
/* Find FIR coefficients, the Fourier transform of the frequency
* response. This is done by dividing the response into a sequence
* of nBands rectangular frequency blocks, each of a different level.
* We can precalculate the Fourier transform for each rectangular band.
* The linearity of the Fourier transform allows us to sum the transforms
* of the individual blocks to get pre-windowed coefficients. As follows
*
* Numbering example for nFIR==199:
* Subscript 0 to 98 is 99 taps; 100 to 198 is 99 taps; 99+1+99=199 taps
* The center coef ( for nFIR=199 taps, nHalfFIR=99 ) is a
* special case that comes from sin(0)/0 and treated first:
*/
cf32f[nHalfFIR] = 2.0f*(aVolts[0]*fNorm[0]); // Coefficient "99"
for(i=1; i<nBands; i++) {
cf32f[nHalfFIR] += 2.0f*aVolts[i]*(fNorm[i]-fNorm[i-1]);
}
for (j=1; j<=nHalfFIR; j++) { // Coefficients "100 to 198"
q = MF_PI*(float32_t)j;
// First, deal with the zero frequency end band that is "low-pass."
cf32f[j+nHalfFIR] = aVolts[0]*sinf(fNorm[0]*2.0*q)/q;
// and then the rest of the bands that have low and high frequencies
for(i=1; i<nBands; i++)
cf32f[j+nHalfFIR] += aVolts[i]*( (sinf(fNorm[i]*2.0*q)/q) - (sinf(fNorm[i-1]*2.0*q)/q) );
}
/* At this point, the cf32f[] coefficients are simply truncated sin(x)/x shapes, creating
* very high sidelobe responses. To reduce the sidelobes, a windowing function is applied.
* This has the side affect of increasing the rate of cutoff for sharp frequency changes.
* The only windowing function available here is that of James Kaiser. This has a number
* of desirable features. The tradeoff of sidelobe level versus cutoff rate is variable.
* We specify it in terms of kdb, the highest sidelobe, in dB, next to a sharp cutoff. For
* calculating the windowing vector, we need a parameter beta, found as follows:
*/
if (kdb<0) return ERR_EQ_SIDELOBES;
if (kdb>50)
beta = 0.1102*(kdb-8.7);
else if (kdb>20.96 && kdb<=50.0)
beta = 0.58417*powf((kdb-20.96), 0.4) + 0.07886*(kdb-20.96);
else
beta=0.0;
// Note: i0f is the floating point in & out zero'th order Bessel function (see mathDSP_F32.h)
kbes = 1.0f / mathEqualizer.i0f(beta); // An additional derived parameter used in loop
// Apply the Kaiser window
scaleXj2 = 2.0f/(float32_t)nFIR;
scaleXj2 *= scaleXj2;
for (j=0; j<=nHalfFIR; j++) { // For 199 Taps, this is 0 to 99
xj2 = (int16_t)(0.5f+(float32_t)j);
xj2 = scaleXj2*xj2*xj2;
WindowWt=kbes*(mathEqualizer.i0f(beta*sqrt(1.0-xj2)));
cf32f[nHalfFIR + j] *= WindowWt; // Apply the Kaiser window to upper half
cf32f[nHalfFIR - j] = cf32f[nHalfFIR +j]; // and create the lower half
}
// And fill in the members of fir_inst
arm_fir_init_f32(&fir_inst, nFIR, (float32_t *)cf32f, &StateF32[0], (uint32_t)block_size);
return 0;
}
/* Calculate response in dB. Leave nFreq point result in array rdb[] supplied
* by the calling .INO See Parks and Burris, "Digital Filter Design," p27 (Type 1).
*/
void AudioFilterEqualizer_F32::getResponse(uint16_t nFreq, float32_t *rdb) {
uint16_t i, j;
float32_t bt;
float32_t piOnNfreq;
uint16_t nHalfFIR;
nHalfFIR = (nFIR - 1)/2;
piOnNfreq = MF_PI / (float32_t)nFreq;
for (i=0; i<nFreq; i++) {
bt = cf32f[nHalfFIR];//bt = 0.5f*cf32f[nHalfFIR]; // Center coefficient
for (j=0; j<nHalfFIR; j++) // Add in the others twice, as they are symmetric
bt += 2.0f*cf32f[j]*cosf(piOnNfreq*(float32_t)((nHalfFIR-j)*i));
rdb[i] = 20.0f*log10f(fabsf(bt)); // Convert to dB
}
}