Bob Larkin
Well-known member
I posted an equalizer block here, https://forum.pjrc.com/threads/60928-Audio-Equalizer-using-FIR. Discussion of that block showed that large FIR filters consumed only modest processor time with the T4.x. This block AudioFilterFIRGeneral_I16 explores that idea and makes some new frequency filters available to T3.x and T4.x. In summary, this block has the new elements:
* The number of FIR elements, nFIR, is limited by processor time and .INO supplied storage.
* The FIR filter is specified by an arbitrary frequency response over nFIR/2 points.
* Dynamic range is maximized by using, internally, a floating point FIR filter and a scaling factor.
* The number of FIR taps can be even or odd.
This block includes a function, FIRGeneralNew(), to design FIR filters using the window method with a Kaiser window. This has been tested at 4000 FIR coefficients (FIR taps). The Kaiser window is very effective in allowing tradeoff of transition band for side-lobe level. However, if a different design method is desired, the FIR coefficients can be loaded directly by a function LoadCoeffs(). Details for all this is listed below as the introduction to AudioFilterFIR_I16.h.
The achieved response can be calculated and returned by a function GetResponse().
I first tried to build this block using the ARM Q15 FIR routine. This proved to be awkward in temporary storage for filter design and more importantly, for dynamic range in the update() filtering. Narrow band filters can be achieved with high numbers of FIR taps, but these need careful scaling of the fixed point coefficiens to not loose noise at the low end or overload at the high end. This is difficult with 16-bit fixed point. Using 32-bit floating point internally adds 8-bits of dynamic range that is very useful. Along with the internal floats, I added a constant scale factor that controls the conversion at the end from float to fixed. The best value (32768.0 gives a "gain of 1") depends on the application. Building in the I16 to F32 and F32 to I16 into the update adds only 3.5 microseconds for an 128 points on T4 and 13 microseconds on T3.6.
This block is useful for modest sized FIR filters in that it allows calculates coefficients for an arbitrary response function, specified by an array of dB response numbers. The calculated filter is linear phase (constant delay at all frequencies), but the LoadCoeffs() allows any phase response.
Keep in mind that this FIR filter is not the only tool in the box. FIR filters are elegant in response possibilities, but computationally hungry. Double FFT Convolution filtering, IIR filters and Decimation/Filter/Interpolation should be considered, and there are probably more.
Here is the response of a low-pass filter with 500 taps, showing the general characteristics of the Kaiser window:
The needed math functions are included with the next two listings, so I believe this pair of .h and .cpp files, placed into a personal library folder, AudioFilterFIRGeneral_I16 should work. Eventually the two math functions should be separated. Also, don't miss the many notes in the two files, particularly the top of the .h file. Here are the AudioFilterGeneral_I16.h and .cpp
and a couple of test .INO
Comments, corrections improvements are appreciated. A version of this for the Audette Float system is coming. Bob
* The number of FIR elements, nFIR, is limited by processor time and .INO supplied storage.
* The FIR filter is specified by an arbitrary frequency response over nFIR/2 points.
* Dynamic range is maximized by using, internally, a floating point FIR filter and a scaling factor.
* The number of FIR taps can be even or odd.
This block includes a function, FIRGeneralNew(), to design FIR filters using the window method with a Kaiser window. This has been tested at 4000 FIR coefficients (FIR taps). The Kaiser window is very effective in allowing tradeoff of transition band for side-lobe level. However, if a different design method is desired, the FIR coefficients can be loaded directly by a function LoadCoeffs(). Details for all this is listed below as the introduction to AudioFilterFIR_I16.h.
The achieved response can be calculated and returned by a function GetResponse().
I first tried to build this block using the ARM Q15 FIR routine. This proved to be awkward in temporary storage for filter design and more importantly, for dynamic range in the update() filtering. Narrow band filters can be achieved with high numbers of FIR taps, but these need careful scaling of the fixed point coefficiens to not loose noise at the low end or overload at the high end. This is difficult with 16-bit fixed point. Using 32-bit floating point internally adds 8-bits of dynamic range that is very useful. Along with the internal floats, I added a constant scale factor that controls the conversion at the end from float to fixed. The best value (32768.0 gives a "gain of 1") depends on the application. Building in the I16 to F32 and F32 to I16 into the update adds only 3.5 microseconds for an 128 points on T4 and 13 microseconds on T3.6.
This block is useful for modest sized FIR filters in that it allows calculates coefficients for an arbitrary response function, specified by an array of dB response numbers. The calculated filter is linear phase (constant delay at all frequencies), but the LoadCoeffs() allows any phase response.
Keep in mind that this FIR filter is not the only tool in the box. FIR filters are elegant in response possibilities, but computationally hungry. Double FFT Convolution filtering, IIR filters and Decimation/Filter/Interpolation should be considered, and there are probably more.
Here is the response of a low-pass filter with 500 taps, showing the general characteristics of the Kaiser window:
The needed math functions are included with the next two listings, so I believe this pair of .h and .cpp files, placed into a personal library folder, AudioFilterFIRGeneral_I16 should work. Eventually the two math functions should be separated. Also, don't miss the many notes in the two files, particularly the top of the .h file. Here are the AudioFilterGeneral_I16.h and .cpp
Code:
/*
* AudioFilterFIRGeneral_I16
*
* Created: Bob Larkin W7PUA 20 May 2020
*
* Credit and thanks to PJRC, Paul Stoffregen and Teensy products for the audio
* system and library that this is built upon as well as the float32
* work of Chip Audette embodied in the OpenAudio_ArduinoLibrary. Many thanks
* for the library structures and wonderful Teensy products.
*
* There are enough different FIR filter Audio blocks to need a summary. Here goes:
*
* AudioFilterFIR (Teensy Audio Library by PJRC) handles 16-bit integer data,
* and a maximum of 200 FIR coefficients, even only. (taps). For Teensy Audio.
* AudioFilterFIR_F32 (OpenAudio_ArduinoLibrary by Chip Audette) handles 32-bit floating point
* data and a maximum of 200 taps. Can be used with a mix of Teensy Audio
* and 32-bit OpenAudio_Arduino blocks.
* AudioFilterFIRGeneral_F32 (This block) handles 32-bit floating point
* data and very large number of taps, even or odd. Can be used with a mix of Teensy Audio
* and 32-bit OpenAudio_Arduino blocks. This includes the design of an
* arbitrary frequency response using Kaiser windows.
* AudioFilterFIRGeneral_I16 Same as this block, but data is 16-bit integer and
* the number of taps must be even.
* AudioFilterEqualizer_F32 handles 32-bit floating point data and 250 maximum taps
* even or odd. Can be used with a mix of Teensy Audio
* and 32-bit OpenAudio_Arduino blocks. This includes the design of an
* arbitrary frequency response using multiple flat response steps. A Kaiser windows
* is used.
* AudioFilterEqualizer_I16 handles 16-bit integer data and 250 maximum taps,
* even only.
*
* FIR filters suffer from needing considerable computation of the multiply-and-add
* sort. This limits the number of taps that can be used, but less so as time goes by.
* In particular, the Teensy 4.x, if it *did nothing but* FIR calculations, could
* use about 6000 taps inmonaural, which is a huge number. But, this also
* suggests that if the filtering task is an important function of a project,
* using, say 2000 taps is practical.
*
* FIR filters can be (and are here) implemented to have symmetrical coefficients. This
* results in constant delay at all frequencies (linear phase). For some applications this can
* be an important feature. Sometimes it is suggested that the FIR should not be
* used because of the latency it creates. Note that if constant delay is needed, the FIR
* implementation does this with minimum latency.
*
* For this block, AudioFilterFIRGeneral_F32, memory storage for the FIR
* coefficiients as well as working storage for the ARM FIR routine is provided
* by the calling .INO. This allows large FIR sizes without always tying up a
* big memory block.
*
* This block normally calculates the FIR coefficients using a Fourier transform
* of the desired amplitude response and a Kaiser window. This flexability requires
* the calling .INO to provide an array of response levels, in relative dB,
* that is half the length of the number of FIR taps. An example of this entry is a
* 300 point low-pass filter with a cutoff at 10% of the 44.1 kHz sample frequency:
* for(int i=0; i<150; i++) {
* if (i<30) dbA[i] = 0.0f;
* else dbA[i] = -140.0f;
* }
* firg1.FIRGeneralNew(&dbA[0], 300, &equalizeCoeffs[0], 50.0f, &workSpace[0]);
*
* As an alternate to inputting the response function, the FIR coefficients can be
* entered directly using LoadCoeffs(nFIR, cf32f, *pStateArray). This is a very quick
* operation as only pointers to coefficients are involved. Several filters can be
* stored in arrays and switched quickly this way. If this is done, pStateArray[]
* as initially setup should be large enough for all filters. There will be "clicks"
* associated with filter changes and these may need to be muted.
*
* How well the desired response is achieved depends on the number of FIR coefficients
* being used. As noted above, for some applications it may be desired to use
* large numbers of taps. The achieved response can be evaluated
* by the function getResponse(nPoints, pResponse) which fills an .INO-supplied array
* pResponse[nPoints] with the frequency response of the equalizer in dB. The nPoints
* are spread evenly between 0.0 and half of the sample frequency.
*
* Initialization is a 2-step process. This makes it practical to change equalizer
* levels on-the-fly. The constructor starts up with a 4-tap FIR setup for direct
* pass through. Then the setup() in the .INO can specify the equalizer.
* The newEqualizer() function has several parameters, the number of equalizer bands,
* the frequencies of the bands, and the sidelobe level. All of these can be changed
* dynamically. This function can be changed dynamically, but it may be desireable to
* mute the audio during the change to prevent clicks.
*
* An arbitrary number of FIR coefficients can also be loaded on-the-fly. This may
* involve extra memory storage in the .INO area, but not within this block.
*
* Measured timing of update() for L or R 128 sample block, Teensy 3.6:
* Fixed time 26 microseconds (about 13 due to integer/float and float/integer conversions)
* Per FIR Coefficient time 2.5 microseconds
* Total for 500 FIR Coefficients = 1255 microseconds (40.3% of 44117 Hz available time)
* Total for stereo is twice those numbers.
* Measured timing of update() for L or R 128 sample block, Teensy 4.0:
* Fixed time 5 microseconds (about 3.5 due to integer/float and float/integer conversions)
* Per FIR Coefficient time 0.44 microseconds (about 290E6 multiply and accumulate/sec)
* Total for 500 FIR Coefficients = 223 microseconds (7.7% of 44100 Hz available time)
* Total for stereo is twice those numbers
* Measured for FIRGeneralNew(), T4.0, to design a 4000 tap FIR is 14 sec. This goes
* with the square of the number of taps.
* Measured for getResponse() for nFIR=4000 and nFreq=5000, T4.0, is about a minute.
*
* Functions for the AudioFilterFIRGeneral_F32 object are
* FIRGeneralNew(*adb, nFIR, cf32f, kdb, *pStateArray, scale); // to design FIR and
* make prepare for update()
* LoadCoeffs(nFIR, cf32f, *pStateArray); // To directly load float FIR coefficients
* getResponse(nFreq, *rdb); // To obtain the amplitude response in dB, rdb[] for
* current coefficients
*
* Copyright (c) 2020 Bob Larkin
* Any snippets of code from PJRC or Chip Audette used here brings with it
* the associated license.
*
* In addition, work here is covered by MIT LIcense:
*
* 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.
*/
#ifndef _filter_FIR_general_f32_h
#define _filter_FIR_general_f32_h
#include "Arduino.h"
#include "Audio.h"
#include "AudioStream.h"
#include "arm_math.h"
#ifndef MF_PI
#define MF_PI 3.1415926f
#endif
#ifndef MF_TWOPI
#define MF_TWOPI 6.2831853f
#endif
// Temporary timing test
#define TEST_TIME_FIRG 0
#define ERR_FIRGEN_BANDS 1
#define ERR_FIRGEN_SIDELOBES 2
#define ERR_FIRGEN_NFIR 3
class AudioFilterFIRGeneral_I16 : public AudioStream
{
public:
AudioFilterFIRGeneral_I16(void): AudioStream(1,inputQueueArray) {
// Initialize FIR instance (ARM DSP Math Library) with default simple passthrough FIR
arm_fir_init_f32(&fir_inst, nFIR, (float32_t *)cf32f, &StateF32[0], (uint32_t)128);
}
uint16_t FIRGeneralNew(float32_t *adb, uint16_t _nFIR, float32_t *_cf32f, float32_t kdb, float32_t *pStateArray, float32_t scale);
uint16_t LoadCoeffs(uint16_t _nFIR, float32_t *_cf32f, float32_t *pStateArray);
void getResponse(uint16_t nFreq, float32_t *rdb);
void update(void);
private:
audio_block_t *inputQueueArray[1];
uint16_t block_size = 128;
float32_t firStart[4] = {0.0f, 1.0f, 0.0f, 0.0f}; // Initialize to passthrough
float32_t* cf32f = firStart; // pointer to current coefficients
uint16_t nFIR = 4; // Number of coefficients
float32_t sample_rate_Hz = AUDIO_SAMPLE_RATE;
float32_t scale = 32768.0f;
void float_to_q15_scaled(float32_t*, q15_t*, uint32_t, float32_t);
// *Temporary* - TEST_TIME allows measuring time in microseconds for each part of the update()
#if TEST_TIME_FIRG
elapsedMicros tElapse;
int32_t iitt = 999000; // count up to a million during startup
#endif
// ARM DSP Math library filter instance
arm_fir_instance_f32 fir_inst;
float32_t StateF32[128 + 4];
// This needs to be moved from here and made generally re-usable.
/* float i0f(float x) Returns the modified Bessel function Io(x).
* Algorithm is based on Abromowitz and Stegun, Handbook of Mathematical
* Functions, NBS Applied Math Series #55, Section 9.8.
* All in 32-bit floating point
*/
float i0f(float x) {
float af, bf, cf;
if( (af=fabsf(x)) < 3.75f ) {
cf = x/3.75f;
cf = cf*cf;
bf=1.0f+cf*(3.515623f+cf*(3.089943f+cf*(1.20675f+cf*(0.265973f+
cf*(0.0360768f+cf*0.0045813f)))));
}
else {
cf = 3.75f/af;
bf=(expf(af)/sqrtf(af))*(0.3989423f+cf*(0.0132859f+cf*(0.0022532f+
cf*(-0.0015756f+cf*(0.0091628f+cf*(-0.0205771f+cf*(0.0263554f+
cf*(-0.0164763f+cf*0.0039238f))))))));
}
return bf;
}
};
#endif
Code:
/* AudioFilterFIRGeneralr_I16.cpp
*
* Bob Larkin, W7PUA 30 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_I16.h"
void AudioFilterFIRGeneral_I16::update(void) {
audio_block_t *blockIn, *blockOut;
// We can go without the Audette F32 audio library format if we just
// provide storage for a couple of arrays. This is always 128 data items.
float32_t floatData1[128];
float32_t floatData2[128];
#if TEST_TIME_FIRG
if (iitt++ >1000000) iitt = -10;
uint32_t t1, t2;
t1 = tElapse;
#endif
blockIn = AudioStream::receiveReadOnly();
if (!blockIn) return;
// If there's no coefficient table, give up.
if (cf32f == NULL) {
AudioStream::release(blockIn);
return;
}
blockOut = AudioStream::allocate(); // get a block for the FIR output
if (blockOut) {
arm_q15_to_float (blockIn->data, floatData1, (uint32_t)128);
// Process 128 float with FIR filter
arm_fir_f32(&fir_inst, floatData1, floatData2, 128);
// Convert float to int with scaling
float_to_q15_scaled (floatData2, blockOut->data, (uint32_t)128, scale);
// Results outside of the allowable Q15 range (-32768, 32767) are now saturated.
AudioStream::transmit(blockOut); // send the I16 FIR output
AudioStream::release(blockOut);
}
AudioStream::release(blockIn);
#if TEST_TIME_FIRG
t2 = tElapse;
if(iitt < 0) { Serial.print((int32_t)iitt); Serial.print(" At FIRGeneral end, microseconds = "); Serial.println (t2 - t1); }
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 close-in sidelobes
* kdb=70 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
* scale multiply value for conversion from float to I16 (Q15)
*
* 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_I16::FIRGeneralNew(
float32_t *adb, uint16_t _nFIR, float32_t *_cf32f, float32_t kdb,
float32_t *pStateArray, float32_t _scale) {
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)
bool even;
cf32f = _cf32f; // Make pivate copies
nFIR = _nFIR;
scale = _scale;
// 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: nHalfFIR = 10
* 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(x) is the zero'th order modified Bessel function (see mathDSP_F32.h)
kbes = 1.0f / i0f(beta); // An additional derived parameter used in loop
// 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;
if((1.0f - xn2) < 0.0f) xn2 = 0.999999f; // Potential rounding issues
WindowWt=kbes*(i0f(beta*sqrt(1.0f-xn2)));
if(kdb < 0.1f) WindowWt = 1.0f; // kdb==0.0 means no window
cf32f[nHalfFIR - n - 1] *= WindowWt; // Apply win; win for n=0 goes with cf32f[nHalfFIR-1]
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;
if((1.0f - xn2) < 0.0f) xn2 = 0.999999f;
WindowWt=kbes*(i0f(beta*sqrt(1.0-xn2)));
if(kdb < 0.1f) WindowWt = 1.0f;
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 structure fir_inst used in update().
AudioNoInterrupts();
arm_fir_init_f32(&fir_inst, nFIR, (float32_t *)cf32f, &pStateArray[0], (uint32_t)block_size);
AudioInterrupts();
#if TEST_TIME_FIRG
iitt = 999000;
#endif
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_I16::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();
#if TEST_TIME_FIRG
iitt = 999000;
#endif
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_I16::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));
}
}
}
// Here for now. Needs to be moved out of AudioFilterFIRGeneral_I16.cpp
/* ----------------------------------------------------------------------------
* This is directly from arm_float_to_q15.c with the addition of
* providing a variable scaling factor, scale (was 32768.0 fixed).
*
* The new call is
* void float_to_q15_scaled(
* float32_t * pSrc,
* q15_t * pDst,
* uint32_t blockSize
* float32_t scale);
*
* Bob Larkin 30 May 2020 Original description and legal stuff follow:
* ---------------------
* Copyright (C) 2010-2014 ARM Limited. All rights reserved.
*
* $Date: 19. March 2015
* $Revision: V.1.4.5
*
* Project: CMSIS DSP Library
* Title: arm_float_to_q15.c
*
* Description: Converts the elements of the floating-point vector to Q15 vector.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* - Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* - Neither the name of ARM LIMITED nor the names of its contributors
* may be used to endorse or promote products derived from this
* software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
* ---------------------------------------------------------------------------- */
void AudioFilterFIRGeneral_I16::float_to_q15_scaled(
float32_t * pSrc,
q15_t * pDst,
uint32_t blockSize,
float32_t scale) /* New from ARM routine RSL 30 May 2020 */
{
float32_t *pIn = pSrc; /* Src pointer */
uint32_t blkCnt; /* loop counter */
#ifdef ARM_MATH_ROUNDING
float32_t in;
#endif /* #ifdef ARM_MATH_ROUNDING */
#ifndef ARM_MATH_CM0_FAMILY
/* Run the below code for Cortex-M4 and Cortex-M3 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
#ifdef ARM_MATH_ROUNDING
/* C = A * scale */
/* convert from float to q15 and then store the results in the destination buffer */
in = *pIn++;
in = (in * scale);
in += in > 0.0f ? 0.5f : -0.5f;
*pDst++ = (q15_t) (__SSAT((q31_t) (in), 16));
in = *pIn++;
in = (in * scale);
in += in > 0.0f ? 0.5f : -0.5f;
*pDst++ = (q15_t) (__SSAT((q31_t) (in), 16));
in = *pIn++;
in = (in * scale);
in += in > 0.0f ? 0.5f : -0.5f;
*pDst++ = (q15_t) (__SSAT((q31_t) (in), 16));
in = *pIn++;
in = (in * scale);
in += in > 0.0f ? 0.5f : -0.5f;
*pDst++ = (q15_t) (__SSAT((q31_t) (in), 16));
#else
/* C = A * scale */
/* convert from float to q15 and then store the results in the destination buffer */
*pDst++ = (q15_t) __SSAT((q31_t) (*pIn++ * scale), 16);
*pDst++ = (q15_t) __SSAT((q31_t) (*pIn++ * scale), 16);
*pDst++ = (q15_t) __SSAT((q31_t) (*pIn++ * scale), 16);
*pDst++ = (q15_t) __SSAT((q31_t) (*pIn++ * scale), 16);
#endif /* #ifdef ARM_MATH_ROUNDING */
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
#ifdef ARM_MATH_ROUNDING
/* C = A * scale */
/* convert from float to q15 and then store the results in the destination buffer */
in = *pIn++;
in = (in * scale);
in += in > 0.0f ? 0.5f : -0.5f;
*pDst++ = (q15_t) (__SSAT((q31_t) (in), 16));
#else
/* C = A * scale */
/* convert from float to q15 and then store the results in the destination buffer */
*pDst++ = (q15_t) __SSAT((q31_t) (*pIn++ * scale), 16);
#endif /* #ifdef ARM_MATH_ROUNDING */
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Loop over blockSize number of values */
blkCnt = blockSize;
while(blkCnt > 0u)
{
#ifdef ARM_MATH_ROUNDING
/* C = A * scale */
/* convert from float to q15 and then store the results in the destination buffer */
in = *pIn++;
in = (in * scale);
in += in > 0 ? 0.5f : -0.5f;
*pDst++ = (q15_t) (__SSAT((q31_t) (in), 16));
#else
/* C = A * scale */
/* convert from float to q15 and then store the results in the destination buffer */
*pDst++ = (q15_t) __SSAT((q31_t) (*pIn++ * scale), 16);
#endif /* #ifdef ARM_MATH_ROUNDING */
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0_FAMILY */
}
Code:
/* TestFIRGeneralLarge5_I16.ino Bob Larkin 24 May 2020
*
* This is a test of the AudioFilterFIRGeneral_I16 block for Teensy Audio,
* that has new elements:
* * The FIR filter is specified by an arbitrary frequency response ove nFIR/2 points.
* * The number of FIR elements is limited by processor time and .INO supplied storage.
* * Dynamic range is maximized by using floating point FIR filter and a scaling factor.
* * The number of FIR taps can be even or odd.
*
* This test program passes a signal from the SGTL5000 CODEC through
* the FIR filter and back out through the CODEC.
* This version is for the Teensy Audio Library with I16 data types.
*/
#include "Audio.h"
#include "AudioFilterFIRGeneral_I16.h"
AudioInputI2S i2sIn;
AudioFilterFIRGeneral_I16 firg1;
AudioOutputI2S i2sOut;
AudioConnection patchCord1(i2sIn, 0, firg1, 0);
AudioConnection patchCordD(firg1, 0, i2sOut, 0);
AudioControlSGTL5000 codec;
void setup(void) {
// Following 4 arrays push dynamic memory to about 15% of available on T4.0.
float32_t dbA[2000];
float32_t stateArray[4128];
float32_t equalizeCoeffs[4000];
float32_t dBResponse[5000];
int i;
uint16_t nFIR = 4;
float32_t sidelobes = 45.0f;
AudioMemory(10);
// Following is based on Rabiner, McGonegal and Paul, FWFIR test case, but has
// had the windowing effect divided out to show their basic filter response.
float32_t hpCoeff[55]={
-0.003643388f, -0.007195844f, 0.012732424f, -0.007796006f, -0.004276372f,
0.013760401f, -0.012262941f, 0.000000304f, 0.013553423f, -0.016818445f,
0.005786355f, 0.011693321f, -0.021220577f, 0.013364335f, 0.007566100f,
-0.025227439f, 0.023410856f, -0.000000305f, -0.028612852f, 0.037841329f,
-0.014052131f, -0.031182658f, 0.063661835f, -0.046774701f, -0.032787389f,
0.151364865f, -0.257518316f, 0.300000700f, -0.257518316f, 0.151364865f,
-0.032787389f, -0.046774701f, 0.063661835f, -0.031182658f, -0.014052131f,
0.037841329f, -0.028612852f, -0.000000305f, 0.023410856f, -0.025227439f,
0.007566100f, 0.013364335f, -0.021220577f, 0.011693321f, 0.005786355f,
-0.016818445f, 0.013553423f, 0.000000304f, -0.012262941f, 0.013760401f,
-0.004276372f, -0.007796006f, 0.012732424f, -0.007195844f, -0.003643388};
Serial.begin(300); delay(1000);
Serial.println("*** Test FIR General routine for I16 ***");
codec.enable();
codec.inputSelect(AUDIO_INPUT_LINEIN);
codec.adcHighPassFilterDisable();
codec.lineInLevel(2);
codec.volume(0.8);
// Un-comment one of the following 5, or use the direct Coefficient load that follows
// Test case for odd nFIR from Burris and Parks Example 3.1 with Kaiser window
// nFIR = 21; sidelobes = 45.0f;
// dbA[0]=0.0; dbA[1]=0.0; dbA[2]=0.0; dbA[3]=0.0; dbA[4]=0.0; dbA[5]=0.0;
// dbA[6]=-140.0; dbA[7]=-140.0; dbA[8]=-140.0; dbA[9]=-140.0; dbA[10]=-140.0;
// Test case for even nFIR from Burris and Parks Example 3.2 with Kaiser window
// nFIR can be even or odd with AudioFilterFIRGeneral_I16
// nFIR = 20; sidelobes = 45.0f;
// dbA[0]=0.0; dbA[1]=0.0; dbA[2]=0.0; dbA[3]=0.0; dbA[4]=0.0; dbA[5]=-120.0;
// dbA[6]=-120.0; dbA[7]=-120.0; dbA[8]=-120.0; dbA[9]=-120.0;
// A simple, but sharp cutoff, 500-tap low-pass filter with 80 dB first sidelobe.
nFIR = 500; sidelobes = 80;
for(i=0; i<250; i++) {
if (i<40) dbA[i] = 0.0f; // fco about 22050*40/250, about 3500 Hz
else dbA[i] = -140.0f;
}
// The next example tests extremely large FIR arrays and *only for Teensy 4.x*
// Narrow band "CW" filter, tried with 60 dB sidelobes (60 to 80 or 70 to 72
// The dynamic range management on a filter like this can be challenging.
// Be aware that computing the coefficients at setup() takes several seconds.
// nFIR = 4000; sidelobes = 60; // 3999 to try odd, 4000 for even
// for(i=0; i<2000; i++) {
// if (i<70 || i>72) dbA[i] = -120.0f;
// else dbA[i] = 0.0f;
// }
// Another extreme case, again only for T4.x.
// Generate a response curve for our "octave" equalizer, tried here with 30 dB sidelobes
// nFIR = 3999; sidelobes = 30.0f;
// dbA[0] = 12.0f; // Avoid log(0)
// for(i=1; i<2000; i++)
// dbA[i] = 12.0f*cosf(10.0f*log10f(11.02925f*(float32_t)i));
// Initialize the FIR filter design and run
uint16_t eq = firg1.FIRGeneralNew(&dbA[0], nFIR, &equalizeCoeffs[0], sidelobes, &stateArray[0], 16384.0f);
if (eq == ERR_FIRGEN_BANDS)
Serial.println("FIR General failed: Invalid number of frequency points.");
else if (eq == ERR_FIRGEN_SIDELOBES)
Serial.println("FIR General failed: Invalid sidelobe specification.");
else if (eq == ERR_FIRGEN_NFIR)
Serial.println("FIR General failed: Invalid number of FIR coefficients.");
else
Serial.println("FIR General initialized successfully.");
// Load (if not commented) non-windowed HP filter *float* coefficients for test case:
// firg1.LoadCoeffs(55, hpCoeff, workSpace); // Direct load of FIR coefficients, already calculated
Serial.println("FIR Coeffs");
for(i=0; i<nFIR; i++)
Serial.println(equalizeCoeffs[i], 7);
Serial.println(" -------");
// Get frequency response in dB for 500 points, uniformly spaced from 0 to 22050 Hz
firg1.getResponse(500, dBResponse);
Serial.println("Freq Hz, Response dB");
for(int m=0; m<500; m++) { // Print the response in Hz, dB, suitable for a spread sheet
Serial.print((float32_t)m * 22050.0f / 500.0f);
Serial.print(",");
Serial.println(dBResponse[m]);
}
}
void loop(void) {
}
Code:
/* TestFIRGeneralLarge4.ino Bob Larkin 24 May 2020
* Test the generation of FIR filters and obtaining their
* responses. See TestFIRGeneralLarge5.ino to run the filters.
* This is a test of the AudioFilterFIR_I16 for Teensy Audio.
*/
#include "Audio.h"
#include "AudioFilterFIRGeneral_I16.h"
AudioInputI2S i2s1; // Creates timing
AudioFilterFIRGeneral_F32 firg1;
float32_t dbA[2000];
float32_t workSpace[4128];
float32_t equalizeCoeffs[4000];
int i, k;
float32_t dBResponse[5000]; // Extreme size, can show lots of detail!
// Following is based on Rabiner, McGonegal and Paul, FWFIR test case, but has
// had the windowing effect divided out to show their basic filter response.
float32_t hpCoeff[55]={
-0.003643388f, -0.007195844f, 0.012732424f, -0.007796006f, -0.004276372f,
0.013760401f, -0.012262941f, 0.000000304f, 0.013553423f, -0.016818445f,
0.005786355f, 0.011693321f, -0.021220577f, 0.013364335f, 0.007566100f,
-0.025227439f, 0.023410856f, -0.000000305f, -0.028612852f, 0.037841329f,
-0.014052131f, -0.031182658f, 0.063661835f, -0.046774701f, -0.032787389f,
0.151364865f, -0.257518316f, 0.300000700f, -0.257518316f, 0.151364865f,
-0.032787389f, -0.046774701f, 0.063661835f, -0.031182658f, -0.014052131f,
0.037841329f, -0.028612852f, -0.000000305f, 0.023410856f, -0.025227439f,
0.007566100f, 0.013364335f, -0.021220577f, 0.011693321f, 0.005786355f,
-0.016818445f, 0.013553423f, 0.000000304f, -0.012262941f, 0.013760401f,
-0.004276372f, -0.007796006f, 0.012732424f, -0.007195844f, -0.003643388};
void setup(void) {
uint16_t nFIR = 4;
float32_t sidelobes = 45;
AudioMemory(5);
AudioMemory_F32(10);
Serial.begin(300); delay(1000);
Serial.println("*** Test Response of FIR General routine for F32 ***");
// Un-comment one of the following, or use the direct Coefficient load that follows
// Test case for odd nFIR from Burris and Parks Example 3.1
nFIR = 21; sidelobes = 0.0; // No window
dbA[0]=0.0; dbA[1]=0.0; dbA[2]=0.0; dbA[3]=0.0; dbA[4]=0.0; dbA[5]=0.0;
dbA[6]=-140.0; dbA[7]=-140.0; dbA[8]=-140.0; dbA[9]=-140.0; dbA[10]=-140.0;
// Test case for even nFIR from Burris and Parks Example 3.2
// nFIR = 20; sidelobes = 0.0;
// dbA[0]=0.0; dbA[1]=0.0; dbA[2]=0.0; dbA[3]=0.0; dbA[4]=-120.0; dbA[5]=-120.0;
// dbA[6]=-120.0; dbA[7]=-120.0; dbA[8]=-120.0; dbA[9]=-120.0;
// Test case for odd nFIR from Burris and Parks Example 3.1 with Kaiser Window
// nFIR = 21; sidelobes = 45.0f;
// dbA[0]=0.0; dbA[1]=0.0; dbA[2]=0.0; dbA[3]=0.0; dbA[4]=0.0; dbA[5]=0.0;
// dbA[6]=-140.0; dbA[7]=-140.0; dbA[8]=-140.0; dbA[9]=-140.0; dbA[10]=-140.0f;
// Test case for even nFIR from Burris and Parks Example 3.2
// nFIR = 20; sidelobes = 45.0f;
// dbA[0]=0.0; dbA[1]=0.0; dbA[2]=0.0; dbA[3]=0.0; dbA[4]=0.0; dbA[5]=-120.0;
// dbA[6]=-120.0; dbA[7]=-120.0; dbA[8]=-120.0; dbA[9]=-120.0;
// Rabiner, McGonegal and Paul, FWFIR test case, HP Kaiser SL=60, fco=0.35
// nFIR = 55; sidelobes = 60.0;
// for(i=0; i<28; i++) {
// if (i<20) dbA[i] = -120.0f;
// else dbA[i] = 0.0f;
// }
// The next 3 examples are tests of extremely large FIR arrays and only for Teensy 4
// Generate a response curve for our "octave" equalizer, tried with 30 dB sidelobes
// nFIR = 3999; sidelobes = 30.0f;
// dbA[0] = 12.0f; // Avoid log(0)
// for(i=1; i<2000; i++)
// dbA[i] = 12.0f*cosf(10.0f*log10f(11.02925f*(float32_t)i));
// Generate a slowly changing response, up 12 dB at bass and treble and -12 dB
// for mid-frequencies. Scaled to "octaves" by the form cos(log(f))
// nFIR = 3999; sidelobes = 30.0f;
// dbA[0] = 12.0f;
// dbA[1] = 12.0f;
// for(i=2; i<2000; i++)
// dbA[i] = -12.0f*cosf(2.2f*log10f(11.02925f*(float32_t)i));
// Narrow band "CW" filter, tried with 60 dB sidelobes (60 to 80 or 70 to 72
// nFIR = 3999; sidelobes = 60;
// for(i=0; i<2000; i++) {
// if (i<70 || i>72) dbA[i] = -120.0f;
// else dbA[i] = 0.0f;
// }
// Initialize the FIR filter design and run
uint16_t eq = firg1.FIRGeneralNew(&dbA[0], nFIR, &equalizeCoeffs[0], sidelobes, &workSpace[0]);
if (eq == ERR_EQ_BANDS)
Serial.println("FIR General failed: Invalid number of frequency points.");
else if (eq == ERR_EQ_SIDELOBES)
Serial.println("FIR General failed: Invalid sidelobe specification.");
else if (eq == ERR_EQ_NFIR)
Serial.println("FIR General failed: Invalid number of FIR coefficients.");
else
Serial.println("FIR General initialized successfully.");
// Load (if not commented) non-windowed HP filter coefficients, test case:
// firg1.LoadCoeffs(55, hpCoeff, workSpace); // Direct load of FIR coefficients, already calculated
Serial.println("FIR Coeffs");
for(i=0; i<nFIR; i++)
Serial.println(equalizeCoeffs[i], 7);
Serial.println("");
// Get frequency response in dB for 5000 points, uniformly spaced from 0 to 22058 Hz
// This is chosen at 5000 for test. Normally, a smaller value,
// perhaps 500, would be adequate.
firg1.getResponse(5000, dBResponse);
Serial.println("Freq Hz, Response dB");
for(int m=0; m<5000; m++) { // Print the response in Hz, dB, suitable for a spread sheet
// Uncomment/comment next 3 for print/no print
Serial.print((float32_t)m * 22058.5f / 5000.0f);
Serial.print(",");
Serial.println(dBResponse[m]);
}
}
void loop(void) {
}Comments, corrections improvements are appreciated. A version of this for the Audette Float system is coming. Bob
