You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
193 lines
8.2 KiB
193 lines
8.2 KiB
4 years ago
|
/* 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
|
||
|
|
}
|
||
|
|
}
|