123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294 |
- #ifndef BELL_BIQUADFILTER_H
- #define BELL_BIQUADFILTER_H
- #include <mutex>
- #include <cmath>
- #include "esp_platform.h"
- extern "C" int dsps_biquad_f32_ae32(const float* input, float* output, int len, float* coef, float* w);
- class BiquadFilter
- {
- private:
- std::mutex processMutex;
- float coeffs[5];
- float w[2] = {0, 0};
- public:
- BiquadFilter(){};
- // Generates coefficients for a high pass biquad filter
- void generateHighPassCoEffs(float f, float q){
- if (q <= 0.0001) {
- q = 0.0001;
- }
- float Fs = 1;
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = (1 + c) / 2;
- float b1 = -(1 + c);
- float b2 = b0;
- float a0 = 1 + alpha;
- float a1 = -2 * c;
- float a2 = 1 - alpha;
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for a low pass biquad filter
- void generateLowPassCoEffs(float f, float q){
- if (q <= 0.0001) {
- q = 0.0001;
- }
- float Fs = 1;
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = (1 - c) / 2;
- float b1 = 1 - c;
- float b2 = b0;
- float a0 = 1 + alpha;
- float a1 = -2 * c;
- float a2 = 1 - alpha;
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for a high shelf biquad filter
- void generateHighShelfCoEffs(float f, float gain, float q)
- {
- if (q <= 0.0001)
- {
- q = 0.0001;
- }
- float Fs = 1;
- float A = sqrtf(pow(10, (double)gain / 20.0));
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = A * ((A + 1) + (A - 1) * c + 2 * sqrtf(A) * alpha);
- float b1 = -2 * A * ((A - 1) + (A + 1) * c);
- float b2 = A * ((A + 1) + (A - 1) * c - 2 * sqrtf(A) * alpha);
- float a0 = (A + 1) - (A - 1) * c + 2 * sqrtf(A) * alpha;
- float a1 = 2 * ((A - 1) - (A + 1) * c);
- float a2 = (A + 1) - (A - 1) * c - 2 * sqrtf(A) * alpha;
- std::lock_guard lock(processMutex);
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for a low shelf biquad filter
- void generateLowShelfCoEffs(float f, float gain, float q)
- {
- if (q <= 0.0001)
- {
- q = 0.0001;
- }
- float Fs = 1;
- float A = sqrtf(pow(10, (double)gain / 20.0));
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = A * ((A + 1) - (A - 1) * c + 2 * sqrtf(A) * alpha);
- float b1 = 2 * A * ((A - 1) - (A + 1) * c);
- float b2 = A * ((A + 1) - (A - 1) * c - 2 * sqrtf(A) * alpha);
- float a0 = (A + 1) + (A - 1) * c + 2 * sqrtf(A) * alpha;
- float a1 = -2 * ((A - 1) + (A + 1) * c);
- float a2 = (A + 1) + (A - 1) * c - 2 * sqrtf(A) * alpha;
- std::lock_guard lock(processMutex);
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for a notch biquad filter
- void generateNotchCoEffs(float f, float gain, float q)
- {
- if (q <= 0.0001)
- {
- q = 0.0001;
- }
- float Fs = 1;
- float A = sqrtf(pow(10, (double)gain / 20.0));
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = 1 + alpha * A;
- float b1 = -2 * c;
- float b2 = 1 - alpha * A;
- float a0 = 1 + alpha;
- float a1 = -2 * c;
- float a2 = 1 - alpha;
- std::scoped_lock lock(processMutex);
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for a peaking biquad filter
- void generatePeakCoEffs(float f, float gain, float q)
- {
- if (q <= 0.0001) {
- q = 0.0001;
- }
- float Fs = 1;
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = alpha;
- float b1 = 0;
- float b2 = -alpha;
- float a0 = 1 + alpha;
- float a1 = -2 * c;
- float a2 = 1 - alpha;
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for a peaking EQ biquad filter
- void generatePeakingEqCoEffs(float f, float gain, float q)
- {
- // formular taken from: https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
- if (q <= 0.0001) {
- q = 0.0001;
- }
- float Fs = 1;
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float A = sqrtf(pow(10, (double)gain / 20.0));
- float b0 = 1 + (alpha*A);
- float b1 = (-2*c);
- float b2 = 1 - (alpha*A);
- float a0 = 1 + (alpha/A);
- float a1 = b1;
- float a2 = 1 - (alpha/A);
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for an all pass 180° biquad filter
- void generateAllPass180CoEffs(float f, float q)
- {
- if (q <= 0.0001) {
- q = 0.0001;
- }
- float Fs = 1;
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = 1 - alpha;
- float b1 = -2 * c;
- float b2 = 1 + alpha;
- float a0 = 1 + alpha;
- float a1 = -2 * c;
- float a2 = 1 - alpha;
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- // Generates coefficients for an all pass 360° biquad filter
- void generateAllPass360CoEffs(float f, float q)
- {
- if (q <= 0.0001) {
- q = 0.0001;
- }
- float Fs = 1;
- float w0 = 2 * M_PI * f / Fs;
- float c = cosf(w0);
- float s = sinf(w0);
- float alpha = s / (2 * q);
- float b0 = 1 - alpha;
- float b1 = -2 * c;
- float b2 = 1 + alpha;
- float a0 = 1 + alpha;
- float a1 = -2 * c;
- float a2 = 1 - alpha;
- coeffs[0] = b0 / a0;
- coeffs[1] = b1 / a0;
- coeffs[2] = b2 / a0;
- coeffs[3] = a1 / a0;
- coeffs[4] = a2 / a0;
- }
- void processSamples(float *input, int numSamples)
- {
- std::scoped_lock lock(processMutex);
- #ifdef ESP_PLATFORM
- dsps_biquad_f32_ae32(input, input, numSamples, coeffs, w);
- #else
- // Apply the set coefficients
- for (int i = 0; i < numSamples; i++)
- {
- float d0 = input[i] - coeffs[3] * w[0] - coeffs[4] * w[1];
- input[i] = coeffs[0] * d0 + coeffs[1] * w[0] + coeffs[2] * w[1];
- w[1] = w[0];
- w[0] = d0;
- }
- #endif
- }
- };
- #endif
|