/* AudioFilterFIRGeneralr_F32.cpp
*
* Bob Larkin, W7PUA 20 May 2020 (c)
*
* 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.
*/
/* A math note - The design of the FIR filter from the frequency response requires
* taking a discrete Fourier transform. The calculation of the achieved
* frequency response requires another Fourier transform. Good news is
* that this is arithmetically simple, because of
* working with real (not complex) inputs and requiring the FIR be linear
* phase. One of a number of references is C.S. Burris and T. W. Parks,
* "Digital Filter Design." That book also has excellent sections on
* Chebychev error (Parks-McClellan-Rabiner) FIR filters and IIR Filters.
*/
#include "AudioFilterFIRGeneral_F32.h"
void AudioFilterFIRGeneral_F32::update(void) {
audio_block_f32_t *blockIn, *blockOut;
#if TEST_TIME_FIRG
if (iitt++ >1000000) iitt = -10;
uint32_t t1, t2;
t1 = tElapse;
#endif
blockIn = AudioStream_F32::receiveReadOnly_f32();
if (!blockIn) return;
// If there's no coefficient table, give up.
if (cf32f == NULL) {
AudioStream_F32::release(blockIn);
return;
}
blockOut = AudioStream_F32::allocate_f32(); // get a block for the FIR output
if (blockOut) {
// The FIR update
arm_fir_f32(&fir_inst, blockIn->data, blockOut->data, blockIn->length);
AudioStream_F32::transmit(blockOut); // send the FIR output
AudioStream_F32::release(blockOut);
}
AudioStream_F32::release(blockIn);
#if TEST_TIME_FIRG
t2 = tElapse;
if(iitt++ < 0) {Serial.print("At FIRGeneral end, microseconds = "); Serial.println (t2 - t1); // }
Serial.print("numtaps = "); Serial.println (fir_inst.numTaps);
}
t1 = tElapse;
#endif
}
/* FIRGeneralNew() calculates the generalFIR filter coefficients. Works from:
* adb Pointer to nFIR/2 array adb[] of levels, in dB
* nFIR The number of FIR coefficients (taps) used
* cf32f Pointer to an array of float to hold nFIR 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
* pStateArray Pointer to 128+nFIR array of floats, used here briefly and then
* passed to the FIR routine as working storage
*
* The arrays, adb[], cf32f[] and pStateArray[] are supplied by the calling .INO
*
* Returns: 0 if successful, or an error code if not.
* Errors: 1 = NU
* 2 = sidelobe level out of range, must be > 0
* 3 = nFIR out of range
*
* Note - This function runs at setup time, so slowness is not an issue
*/
uint16_t AudioFilterFIRGeneral_F32::FIRGeneralNew(
float32_t *adb, uint16_t _nFIR, float32_t *_cf32f, float32_t kdb,
float32_t *pStateArray) {
uint16_t i, k, n;
uint16_t nHalfFIR;
float32_t beta, kbes;
float32_t num, xn2, scaleXn2, WindowWt;
float32_t M; // Burris and Parks, (2.16)
mathDSP_F32 mathEqualizer; // For Bessel function
bool even;
cf32f = _cf32f; // Make pivate copies
nFIR = _nFIR;
// Check range of nFIR
if (nFIR<4)
return ERR_FIRGEN_NFIR;
M = 0.5f*(float32_t)(nFIR - 1);
// The number of FIR coefficients is even or odd
if (2*(nFIR/2) == nFIR) {
even = true;
nHalfFIR = nFIR/2;
}
else {
even = false;
nHalfFIR = (nFIR - 1)/2;
}
for (i=0; i<nFIR; i++) // To be sure, zero the coefficients
cf32f[i] = 0.0f;
for(i=0; i<(nFIR+AUDIO_BLOCK_SAMPLES); i++) // And the storage
pStateArray[i] = 0.0f;
// Convert dB to "Voltage" ratios, frequencies to fractions of sampling freq
// Borrow pStateArray, as it has not yet gone to ARM math
for(i=0; i<=nHalfFIR; i++)
pStateArray[i] = powf(10.0, 0.05f*adb[i]);
/* Find FIR coefficients, the Fourier transform of the frequency
* response. This general frequency description has half as many
* frequency dB points as FIR coefficients. If nFIR is odd,
* the number of frequency points is nHalfFIR = (nFIR - 1)/2.
*
* Numbering example for nFIR==21:
* Odd nFIR: Subscript 0 to 9 is 10 taps; 11 to 20 is 10 taps; 10+1+10=21 taps
*
* Numbering example for nFIR==20:
* Even nFIR: Subscript 0 to 9 is 10 taps; 10 to 19 is 10 taps; 10+10=20 taps
*/
float32_t oneOnNFIR = 1.0f / (float32_t)nFIR;
if(even) {
for(n=0; n<nHalfFIR; n++) { // Over half of even nFIR FIR coeffs
cf32f[n] = pStateArray[0]; // DC term
for(k=1; k<nHalfFIR; k++) { // Over all frequencies, half of nFIR
num = (M - (float32_t)n) * (float32_t)k;
cf32f[n] += 2.0f*pStateArray[k]*cosf(MF_TWOPI*num*oneOnNFIR);
}
cf32f[n] *= oneOnNFIR; // Divide by nFIR
}
}
else { // nFIR is odd
for(n=0; n<=nHalfFIR; n++) { // Over half of FIR coeffs, nFIR odd
cf32f[n] = pStateArray[0];
for(k=1; k<=nHalfFIR; k++) {
num = (float32_t)((nHalfFIR - n)*k);
cf32f[n] += 2.0f*pStateArray[k]*cosf(MF_TWOPI*num*oneOnNFIR);
}
cf32f[n] *= oneOnNFIR;
}
}
/* At this point, the cf32f[] coefficients are simply truncated, creating
* 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 transition.
* The only windowing function available here is that of James Kaiser. This has a number
* of desirable features. The sidelobes drop off as the frequency away from a transition.
* Also, the tradeoff of sidelobe level versus cutoff rate is variable.
* Here 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.0f) return ERR_FIRGEN_SIDELOBES;
if (kdb < 20.0f)
beta = 0.0;
else
beta = -2.17+0.17153*kdb-0.0002841*kdb*kdb; // Within a dB or so
// Note: i0f is the fp zero'th order modified Bessel function (see mathDSP_F32.h)
kbes = 1.0f / mathEqualizer.i0f(beta); // An additional derived parameter used in loop
scaleXn2 = 4.0f / ( ((float32_t)nFIR - 1.0f)*((float32_t)nFIR - 1.0f) ); // Needed for even & odd
if (even) {
// nHalfFIR = nFIR/2; for nFIR even
for (n=0; n<nHalfFIR; n++) { // For 20 Taps, this is 0 to 9
xn2 = 0.5f+(float32_t)n;
xn2 = scaleXn2*xn2*xn2;
WindowWt=kbes*(mathEqualizer.i0f(beta*sqrt(1.0-xn2)));
if(kdb > 0.1f) // kdb==0.0 means no window
cf32f[nHalfFIR - n - 1] *= WindowWt; // Apply window, reverse subscripts
cf32f[nHalfFIR + n] = cf32f[nHalfFIR - n - 1]; // and create the upper half
}
}
else { // nFIR is odd, nHalfFIR = (nFIR - 1)/2
for (n=0; n<=nHalfFIR; n++) { // For 21 Taps, this is 0 to 10, including center
xn2 = (int16_t)(0.5f+(float32_t)n);
xn2 = scaleXn2*xn2*xn2;
WindowWt=kbes*(mathEqualizer.i0f(beta*sqrt(1.0-xn2)));
if(kdb > 0.1f)
cf32f[nHalfFIR - n] *= WindowWt;
// 21 taps, for n=0, nHalfFIR-n = 10, for n=1 nHalfFIR-n=9, for n=nHalfFIR, nHalfFIR-n=0
cf32f[nHalfFIR + n] = cf32f[nHalfFIR - n]; // and create upper half (rewrite center is OK)
}
}
// And finally, fill in the members of fir_inst given in update() to the ARM FIR routine.
AudioNoInterrupts();
arm_fir_init_f32(&fir_inst, nFIR, (float32_t *)cf32f, &pStateArray[0], (uint32_t)block_size);
AudioInterrupts();
return 0;
}
// FIRGeneralLoad() allows an array of nFIR FIR coefficients to be loaded. They come from an .INO
// supplied array. Also, pStateArray[] is .INO supplied and must be (block_size + nFIR) in size.
uint16_t AudioFilterFIRGeneral_F32::LoadCoeffs(uint16_t _nFIR, float32_t *_cf32f, float32_t *pStateArray) {
nFIR = _nFIR;
cf32f = _cf32f;
if (nFIR<4) // Check range of nFIR
return ERR_FIRGEN_NFIR;
for(int i=0; i<(nFIR+AUDIO_BLOCK_SAMPLES); i++) // Zero, to be sure
pStateArray[i] = 0.0f;
AudioNoInterrupts();
arm_fir_init_f32(&fir_inst, nFIR, &cf32f[0], &pStateArray[0], (uint32_t)block_size);
AudioInterrupts();
return 0;
}
/* Calculate frequency response in dB. Leave nFreq point result in array rdb[] supplied
* by the calling .INO See B&P p27 (Type 1 and 2). Be aware that if nFIR*nFreq is big,
* like 100,000 or more, this will take a while to calcuulate all the cosf(). Normally,
* this is not an issue as this is an infrequent calculation.
* This function assumes that the phase of the FIR is linear with frequency, i.e.,
* the coefficients are symmetrical about the middle. Otherwise, doubling of values,
* as is done here, is not valid.
*/
void AudioFilterFIRGeneral_F32::getResponse(uint16_t nFreq, float32_t *rdb) {
uint16_t i, n;
float32_t bt;
float32_t piOnNfreq;
uint16_t nHalfFIR;
float32_t M;
piOnNfreq = MF_PI / (float32_t)nFreq;
// The number of FIR coefficients, even or odd?
if (2*(nFIR/2) == nFIR) { // it is even
nHalfFIR = nFIR/2;
M = 0.5f*(float32_t)(nFIR - 1);
for (i=0; i<nFreq; i++) {
bt = 0.0;
for (n=0; n<nHalfFIR; n++) // Add in terms twice, as they are symmetric
bt += 2.0f*cf32f[n]*cosf(piOnNfreq*((M-(float32_t)n)*(float32_t)i));
rdb[i] = 20.0f*log10f(fabsf(bt)); // Convert to dB
}
}
else { // it is odd
nHalfFIR = (nFIR - 1)/2;
for (i=0; i<nFreq; i++) {
bt = cf32f[nHalfFIR]; // Center coefficient
for (n=0; n<nHalfFIR; n++) // Add in the others twice, as they are symmetric
bt += 2.0f*cf32f[n]*cosf(piOnNfreq*(float32_t)((nHalfFIR-n)*i));
rdb[i] = 20.0f*log10f(fabsf(bt));
}
}
}