diff --git a/src/filter_equalizer_F32.cpp b/src/filter_equalizer_F32.cpp new file mode 100644 index 0000000..91df8d4 --- /dev/null +++ b/src/filter_equalizer_F32.cpp @@ -0,0 +1,200 @@ +/* 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 "filter_equalizer_F32.h" + +void AudioFilterEqualizer_HX_F32::update(void) +{ + audio_block_f32_t *block, *block_new; + + + block = AudioStream_F32::receiveReadOnly_f32(); + if (!block) + return; + if (bp) // bypass mode + { + AudioStream_F32::transmit(block); + AudioStream_F32::release(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); + +} + +/* 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. + */ +AudioFilterEqualizer_HX_F32::eq_result_t AudioFilterEqualizer_HX_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 > nfir_max) + return EQ_RESULT_ERR_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 EQ_RESULT_ERR_BANDS; + for (i = 0; i < nBands; i++) + { + aVolts[i] = powf(10.0f, (0.05f * 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.0f * 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.0f * q) / q) - (sinf(fNorm[i - 1] * 2.0f * 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.0f) + return EQ_RESULT_ERR_SIDELOBES; + if (kdb > 50.0f) + beta = 0.1102f * (kdb - 8.7f); + else if (kdb > 20.96f && kdb <= 50.0f) + beta = 0.58417f * powf((kdb - 20.96f), 0.4f) + 0.07886f * (kdb - 20.96f); + else + beta = 0.0f; + // 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.0f - 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 EQ_ERSULT_OK; +} + +/* 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_HX_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 + } +} diff --git a/src/filter_equalizer_F32.h b/src/filter_equalizer_F32.h new file mode 100644 index 0000000..23a8c1d --- /dev/null +++ b/src/filter_equalizer_F32.h @@ -0,0 +1,182 @@ +/* + * AudioFilterEqualizer_HX_F32 + * + * Created: Bob Larkin W7PUA 8 May 2020 + * + * This is a direct translation of the receiver audio equalizer written + * by this author for the open-source DSP-10 radio in 1999. See + * http://www.janbob.com/electron/dsp10/dsp10.htm and + * http://www.janbob.com/electron/dsp10/uhf3_35a.zip + * + * 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. + * + * This equalizer is specified by an array of 'nBands' frequency bands + * each of of arbitrary frequency span. The first band always starts at + * 0.0 Hz, and that value is not entered. Each band is specified by the upper + * frequency limit to the band. + * The last band always ends at half of the sample frequency, which for 44117 Hz + * sample frequency would be 22058.5. Each band is specified by its upper + * frequency in an .INO supplied array feq[]. The dB level of that band is + * specified by a value, in dB, arranged in an .INO supplied array + * aeq[]. Thus a trivial bass/treble control might look like: + * nBands = 3; + * feq[] = {300.0, 1500.0, 22058.5}; + * float32_t bass = -2.5; // in dB, relative to anything + * float32_t treble = 6.0; + * aeq[] = {bass, 0.0, treble}; + * + * It may be obvious that this equalizer is a more general case of the common + * functions such as low-pass, band-pass, notch, etc. For instance, a pair + * of band pass filters would look like: + * nBands = 5; + * feq[] = {500.0, 700.0, 2000.0, 2200.0, 22058.5}; + * aeq[] = {-100.0, 0.0, -100.0, 2.0, -100.0}; + * where we added 2 dB of gain to the 2200 to 2400 Hz filter, relative to the 500 + * to 700 Hz band. + * + * An octave band equalizer is made by starting at some low frequency, say 40 Hz for the + * first band. The lowest frequency band will be from 0.0 Hz up to that first frequency. + * Next multiply the first frequency by 2, creating in our example, a band from 40.0 + * to 80 Hz. This is continued until the last frequency is about 22058 Hz. + * This works out to require 10 bands, as follows: + * nBands = 10; + * feq[] = { 40.0, 80.0, 160.0, 320.0, 640.0, 1280.0, 2560.0, 5120.0, 10240.0, 22058.5}; + * aeq[] = { 5.0, 4.0, 2.0, -3.0, -4.0, -1.0, 3.0, 6.0, 3.0, 0.5 }; + * + * For a "half octave" equalizer, multiply each upper band limit by the square root of 2 = 1.414 + * to get the next band limit. For that case, feq[] would start with a sequence + * like 40, 56.56, 80.00, 113.1, 160.0, ... for a total of about 20 bands. + * + * How well all of this is achieved depends on the number of FIR coefficients + * being used. In the Teensy 3.6 / 4.0 the resourses allow a hefty number, + * say 201, of coefficients to be used without stealing all the processor time + * (see Timing, below). The coefficient and FIR memory is sized for a maximum of + * 250 coefficients, but can be recompiled for bigger with the define FIR_MAX_COEFFS. + * To simplify calculations, the number of FIR coefficients should be odd. If not + * odd, the number will be reduced by one, quietly. + * + * If you try to make the bands too narrow for the number of FIR coeffficients, + * the approximation to the desired curve becomes poor. This can all 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. + * + * This 16-bit integer version adjusts the maximum coefficient size to scale16 in the calls + * to both equalizerNew() and getResponse(). Broadband equalizers can work with full-scale + * 32767.0f sorts of levels, where narrow band filtering may need smaller values to + * prevent overload. Experiment and check carefully. Use lower values if there are doubts. + * + * For a pass-through function, something like this (which can be intermixed with fancy equalizers): + * float32_t fBand[] = {10000.0f, 22058.5f}; + * float32_t dbBand[] = {0.0f, 0.0f}; + * equalize1.equalizerNew(2, &fBand[0], &dbBand[0], 4, &equalizeCoeffs[0], 30.0f, 32767.0f); + * + * Measured timing of update() for a 128 sample block, Teensy 3.6: + * Fixed time 13 microseconds + * Per FIR Coefficient time 2.5 microseconds + * Total for 199 FIR Coefficients = 505 microseconds (17.4% of 44117 Hz available time) + * + * Per FIR Coefficient, Teensy 4.0, 0.44 microseconds + * + * 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. + * + * 01.2024 - added bypass subsystem Piotr Zapart www.hexefx.com + * + * + */ + +#ifndef _FILTEREQUALIZER_F32_H_ +#define _FILTEREQUALIZER_F32_H_ + +#include "Arduino.h" +#include "AudioStream_F32.h" +#include "arm_math.h" +#include "mathDSP_F32.h" + +#ifndef MF_PI +#define MF_PI 3.1415926f +#endif + +//#define EQUALIZER_MAX_COEFFS 251 + +class AudioFilterEqualizer_HX_F32 : public AudioStream_F32 +{ + // GUI: inputs:1, outputs:1 //this line used for automatic generation of GUI node + // GUI: shortName:filter_Equalizer +public: + AudioFilterEqualizer_HX_F32(void) : AudioStream_F32(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)block_size); + } + AudioFilterEqualizer_HX_F32(const AudioSettings_F32 &settings) : AudioStream_F32(1, inputQueueArray) + { + block_size = settings.audio_block_samples; + sample_rate_Hz = settings.sample_rate_Hz; + arm_fir_init_f32(&fir_inst, nFIR, (float32_t *)cf32f, &StateF32[0], (uint32_t)block_size); + } + + void bypass_set(bool s) { bp = s;} + bool bypass_tgl() {bp ^=1; return bp;} + typedef enum + { + EQ_ERSULT_OK = 0, + EQ_RESULT_ERR_BANDS, + EQ_RESULT_ERR_SIDELOBES, + EQ_RESULT_ERR_NFIR + }eq_result_t; + + eq_result_t equalizerNew(uint16_t _nBands, float32_t *feq, float32_t *adb, + uint16_t _nFIR, float32_t *_cf32f, float32_t kdb); + void getResponse(uint16_t nFreq, float32_t *rdb); + void update(void); + +private: + audio_block_f32_t *inputQueueArray[1]; + uint16_t block_size = AUDIO_BLOCK_SAMPLES; + static const uint16_t nfir_max = 251; + + 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 + uint16_t nBands = 2; + float32_t sample_rate_Hz = AUDIO_SAMPLE_RATE; + bool bp = false; + + arm_fir_instance_f32 fir_inst; // ARM DSP Math library filter instance + float32_t StateF32[AUDIO_BLOCK_SAMPLES + nfir_max]; // max, max +}; +#endif