From 16e16bb03515827ea2e0d2ed514d0ab5cafc3eca Mon Sep 17 00:00:00 2001 From: Holger Wirtz Date: Fri, 13 Aug 2021 12:33:15 +0200 Subject: [PATCH] Added class for graphic eq. --- MicroDexed.ino | 4 + config.h | 32 ---- sgtl5000_graphic_eq.hpp | 375 +++++++++++++++++++++++++++------------- 3 files changed, 262 insertions(+), 149 deletions(-) diff --git a/MicroDexed.ino b/MicroDexed.ino index a140ddc..9f049e3 100644 --- a/MicroDexed.ino +++ b/MicroDexed.ino @@ -357,6 +357,10 @@ extern void sequencer(void); extern LCDMenuLib2 LCDML; #endif +#ifdef SGTL5000_AUDIO_ENHANCE +BiquadCoef graphic_eq(7); +#endif + extern void getNoteName(char* noteName, uint8_t noteNumber); /*********************************************************************** diff --git a/config.h b/config.h index 09d895d..24601f4 100644 --- a/config.h +++ b/config.h @@ -183,38 +183,6 @@ #define SGTL5000_LINEOUT_LEVEL 29 #endif -#ifdef SGTL5000_AUDIO_ENHANCE - -#define GRAPHIC_EQ_TYPE_0 bq_type_lowpass -#define GRAPHIC_EQ_CENTER_FRQ_0 115.0 -#define GRAPHIC_EQ_Q_0 2.0 - -#define GRAPHIC_EQ_TYPE_1 bq_type_bandpass -#define GRAPHIC_EQ_CENTER_FRQ_1 330.0 -#define GRAPHIC_EQ_Q_1 2.0 - -#define GRAPHIC_EQ_TYPE_2 bq_type_bandpass -#define GRAPHIC_EQ_CENTER_FRQ_2 990.0 -#define GRAPHIC_EQ_Q_2 2.0 - -#define GRAPHIC_EQ_TYPE_3 bq_type_bandpass -#define GRAPHIC_EQ_CENTER_FRQ_3 2000.0 -#define GRAPHIC_EQ_Q_3 2.0 - -#define GRAPHIC_EQ_TYPE_4 bq_type_bandpass -#define GRAPHIC_EQ_CENTER_FRQ_4 4000.0 -#define GRAPHIC_EQ_Q_4 2.0 - -#define GRAPHIC_EQ_TYPE_5 bq_type_bandpass -#define GRAPHIC_EQ_CENTER_FRQ_5 9900.0 -#define GRAPHIC_EQ_Q_5 2.0 - -#define GRAPHIC_EQ_TYPE_6 bq_type_highpass -#define GRAPHIC_EQ_CENTER_FRQ_6 11000.0 -#define GRAPHIC_EQ_Q_6 2.0 - -#endif - //************************************************************************************************* //* UI //************************************************************************************************* diff --git a/sgtl5000_graphic_eq.hpp b/sgtl5000_graphic_eq.hpp index 41b042c..f6e2642 100644 --- a/sgtl5000_graphic_eq.hpp +++ b/sgtl5000_graphic_eq.hpp @@ -3,69 +3,207 @@ #include #include +#define EQ_LOWPASS 0 +#define EQ_HIGHPASS 1 +#define EQ_BANDPASS 2 +#define EQ_NOTCH 3 +#define EQ_PEAK 4 +#define EQ_LOWSHELF 5 +#define EQ_HIGHSHELF 6 + +#define GRAPHIC_EQ_TYPE_0 EQ_HIGHPASS +#define GRAPHIC_EQ_CENTER_FRQ_0 115.0 +#define GRAPHIC_EQ_Q_0 2.0 + +#define GRAPHIC_EQ_TYPE_1 EQ_BANDPASS +#define GRAPHIC_EQ_CENTER_FRQ_1 330.0 +#define GRAPHIC_EQ_Q_1 2.0 + +#define GRAPHIC_EQ_TYPE_2 EQ_BANDPASS +#define GRAPHIC_EQ_CENTER_FRQ_2 990.0 +#define GRAPHIC_EQ_Q_2 2.0 + +#define GRAPHIC_EQ_TYPE_3 EQ_BANDPASS +#define GRAPHIC_EQ_CENTER_FRQ_3 2000.0 +#define GRAPHIC_EQ_Q_3 2.0 + +#define GRAPHIC_EQ_TYPE_4 EQ_BANDPASS +#define GRAPHIC_EQ_CENTER_FRQ_4 4000.0 +#define GRAPHIC_EQ_Q_4 2.0 + +#define GRAPHIC_EQ_TYPE_5 EQ_BANDPASS +#define GRAPHIC_EQ_CENTER_FRQ_5 9900.0 +#define GRAPHIC_EQ_Q_5 2.0 + +#define GRAPHIC_EQ_TYPE_6 EQ_LOWPASS +#define GRAPHIC_EQ_CENTER_FRQ_6 11000.0 +#define GRAPHIC_EQ_Q_6 2.0 + extern AudioControlSGTL5000 sgtl5000_1; -typedef struct biquad_coefficients_s { - float32_t a0; - float32_t a1; - float32_t a2; - float32_t b1; - float32_t b2; -} biquad_coefficients_t; - -typedef struct biquad_params_s { - uint8_t filter_type; - float32_t Fc; - float32_t Q; -} biquad_params_t; - -enum { - bq_type_lowpass = 0, - bq_type_highpass, - bq_type_bandpass, - bq_type_notch, - bq_type_peak, - bq_type_lowshelf, - bq_type_highshelf +class BiquadCoef +{ + public: + BiquadCoef(uint8_t num_bands); + ~BiquadCoef(); + + void set_eq_type(uint8_t band, uint8_t ft); + void set_eq_Fc(uint8_t band, float32_t frq); + void set_eq_Q(uint8_t band, float32_t q); + void set_gain(uint8_t band, float32_t gain); + void get_coef(uint8_t band, int* c); + + private: + void calcBiquadCoefficients(uint8_t band); + + uint8_t num_bands; + + uint8_t *filter_type; + float32_t *Fc; + float32_t *Q; + float32_t *peakGainDB; + + float32_t *a0; + float32_t *a1; + float32_t *a2; + float32_t *b1; + float32_t *b2; }; -#define NUM_GRAPHIC_EQ_BANDS 7 +BiquadCoef::BiquadCoef(uint8_t num_bands) +{ + num_bands = constrain(num_bands, 1, 7); + + sgtl5000_1.eqFilterCount(num_bands); + + filter_type = new uint8_t[num_bands]; + Fc = new float32_t[num_bands]; + Q = new float32_t[num_bands]; + peakGainDB = new float32_t[num_bands]; + a0 = new float32_t[num_bands]; + a1 = new float32_t[num_bands]; + a2 = new float32_t[num_bands]; + b1 = new float32_t[num_bands]; + b2 = new float32_t[num_bands]; -biquad_coefficients_t biquad_coefficients[NUM_GRAPHIC_EQ_BANDS]; -biquad_params_t biquad_params[NUM_GRAPHIC_EQ_BANDS]; + set_eq_type(0, GRAPHIC_EQ_TYPE_0); + set_eq_Fc(0, GRAPHIC_EQ_CENTER_FRQ_0); + set_eq_Q(0, GRAPHIC_EQ_Q_0); + set_gain(0, 0.0); -void set_sgtl5000_7band_eq(uint8_t band, uint8_t filter_type, float32_t Fc, float32_t Q); -void set_sgtl5000_7band_eq_gain(uint8_t band, float32_t peakGainDB); -void setup_sgtl5000_graphic_7band_eq(void); -void calcBiquadCoefficients(biquad_coefficients_t* biquad_coefficients, uint8_t filter_type, float32_t Fc, float32_t Q, float32_t peakGain); + if (num_bands > 1) + { + set_eq_type(1, GRAPHIC_EQ_TYPE_1); + set_eq_Fc(1, GRAPHIC_EQ_CENTER_FRQ_1); + set_eq_Q(1, GRAPHIC_EQ_Q_1); + set_gain(1, 0.0); + } -void set_sgtl5000_7band_eq(uint8_t band, uint8_t filter_type, float32_t Fc, float32_t Q) + if (num_bands > 2) + { + set_eq_type(2, GRAPHIC_EQ_TYPE_2); + set_eq_Fc(2, GRAPHIC_EQ_CENTER_FRQ_2); + set_eq_Q(2, GRAPHIC_EQ_Q_2); + set_gain(2, 0.0); + } + + if (num_bands > 3) + { + set_eq_type(3, GRAPHIC_EQ_TYPE_3); + set_eq_Fc(3, GRAPHIC_EQ_CENTER_FRQ_3); + set_eq_Q(3, GRAPHIC_EQ_Q_3); + set_gain(3, 0.0); + } + + if (num_bands > 4) + { + set_eq_type(4, GRAPHIC_EQ_TYPE_4); + set_eq_Fc(4, GRAPHIC_EQ_CENTER_FRQ_4); + set_eq_Q(4, GRAPHIC_EQ_Q_4); + set_gain(4, 0.0); + } + + if (num_bands > 5) + { + set_eq_type(5, GRAPHIC_EQ_TYPE_5); + set_eq_Fc(5, GRAPHIC_EQ_CENTER_FRQ_5); + set_eq_Q(5, GRAPHIC_EQ_Q_5); + set_gain(5, 0.0); + } + + if (num_bands > 6) + { + set_eq_type(6, GRAPHIC_EQ_TYPE_6); + set_eq_Fc(6, GRAPHIC_EQ_CENTER_FRQ_6); + set_eq_Q(6, GRAPHIC_EQ_Q_6); + set_gain(6, 0.0); + } + + for (uint8_t i = 0; i < num_bands; i++) + { + int tmp[num_bands]; + + calcBiquadCoefficients(i); + get_coef(i, tmp); + sgtl5000_1.eqFilter(i, tmp); + } +} + +BiquadCoef::~BiquadCoef() +{ + ; +} + +void BiquadCoef::set_eq_type(uint8_t band, uint8_t ft) { - biquad_params[band].filter_type = filter_type; - biquad_params[band].Fc = Fc; - biquad_params[band].Q = Q; + int tmp[num_bands]; + + filter_type[band] = ft; + calcBiquadCoefficients(band); + get_coef(band, tmp); + sgtl5000_1.eqFilter(band, tmp); +} - calcBiquadCoefficients(&biquad_coefficients[band], filter_type, Fc, Q, 0.0); +void BiquadCoef::set_eq_Fc(uint8_t band, float32_t frq) +{ + int tmp[num_bands]; - //sgtl5000_1.eqFilter(band, biquad_coefficients); + Fc[band] = frq; + calcBiquadCoefficients(band); + get_coef(band, tmp); + sgtl5000_1.eqFilter(band, tmp); } -void set_sgtl5000_7band_eq_gain(uint8_t band, float32_t peakGainDB) +void BiquadCoef::set_eq_Q(uint8_t band, float32_t q) { - calcBiquadCoefficients(&biquad_coefficients[band], biquad_params[band].filter_type, biquad_params[band].Fc, biquad_params[band].Q, peakGainDB); - //sgtl5000_1.eqFilter(band, biquad_coefficients); + int tmp[num_bands]; + + Q[band] = q; + calcBiquadCoefficients(band); + get_coef(band, tmp); + sgtl5000_1.eqFilter(band, tmp); } -void setup_sgtl5000_graphic_7band_eq(void) +void BiquadCoef::set_gain(uint8_t band, float32_t gain) { - //sgtl5000_1.eqFilterCount(7); // enable 7 bands - set_sgtl5000_7band_eq(0, GRAPHIC_EQ_TYPE_0, GRAPHIC_EQ_CENTER_FRQ_0, GRAPHIC_EQ_Q_0); - set_sgtl5000_7band_eq(1, GRAPHIC_EQ_TYPE_1, GRAPHIC_EQ_CENTER_FRQ_1, GRAPHIC_EQ_Q_1); - set_sgtl5000_7band_eq(2, GRAPHIC_EQ_TYPE_2, GRAPHIC_EQ_CENTER_FRQ_2, GRAPHIC_EQ_Q_2); - set_sgtl5000_7band_eq(3, GRAPHIC_EQ_TYPE_3, GRAPHIC_EQ_CENTER_FRQ_3, GRAPHIC_EQ_Q_3); - set_sgtl5000_7band_eq(4, GRAPHIC_EQ_TYPE_4, GRAPHIC_EQ_CENTER_FRQ_4, GRAPHIC_EQ_Q_4); - set_sgtl5000_7band_eq(5, GRAPHIC_EQ_TYPE_5, GRAPHIC_EQ_CENTER_FRQ_5, GRAPHIC_EQ_Q_5); - set_sgtl5000_7band_eq(6, GRAPHIC_EQ_TYPE_6, GRAPHIC_EQ_CENTER_FRQ_6, GRAPHIC_EQ_Q_6); + int tmp[num_bands]; + + peakGainDB[band] = gain; + calcBiquadCoefficients(band); + get_coef(band, tmp); + sgtl5000_1.eqFilter(band, tmp); +} + +void BiquadCoef::get_coef(uint8_t band, int* c) +{ + if (c != NULL) + { + c[0] = a0[band] * 0x8000; + c[1] = a1[band] * 0x8000; + c[2] = a2[band] * 0x8000; + c[3] = b1[band] * 0x8000; + c[4] = b2[band] * 0x8000; + } } // Taken from https://www.earlevel.com/main/2012/11/26/biquad-c-source-code/ @@ -86,100 +224,103 @@ void setup_sgtl5000_graphic_7band_eq(void) // You may modify and use this source code to create binary code // for your own purposes, free or commercial. // -void calcBiquadCoefficients(biquad_coefficients_t* biquad_coefficients, uint8_t filter_type, float32_t Fc, float32_t Q, float32_t peakGain) +void BiquadCoef::calcBiquadCoefficients(uint8_t band) { + if (band > num_bands) + band = num_bands; + float32_t norm; - float32_t V = pow(10, fabs(peakGain) / 20.0); - float32_t K = tan(M_PI * Fc); - switch (filter_type) { - case bq_type_lowpass: - norm = 1 / (1 + K / Q + K * K); - biquad_coefficients->a0 = K * K * norm; - biquad_coefficients->a1 = 2 * biquad_coefficients->a0; - biquad_coefficients->a2 = biquad_coefficients->a0; - biquad_coefficients->b1 = 2 * (K * K - 1) * norm; - biquad_coefficients->b2 = (1 - K / Q + K * K) * norm; + float32_t V = pow(10, fabs(peakGainDB[band]) / 20.0); + float32_t K = tan(M_PI * Fc[band]); + switch (filter_type[band]) { + case EQ_LOWPASS: + norm = 1 / (1 + K / Q[band] + K * K); + a0[band] = K * K * norm; + a1[band] = 2 * a0[band]; + a2[band] = a0[band]; + b1[band] = 2 * (K * K - 1) * norm; + b2[band] = (1 - K / Q[band] + K * K) * norm; break; - case bq_type_highpass: - norm = 1 / (1 + K / Q + K * K); - biquad_coefficients->a0 = 1 * norm; - biquad_coefficients->a1 = -2 * biquad_coefficients->a0; - biquad_coefficients->a2 = biquad_coefficients->a0; - biquad_coefficients->b1 = 2 * (K * K - 1) * norm; - biquad_coefficients->b2 = (1 - K / Q + K * K) * norm; + case EQ_HIGHPASS: + norm = 1 / (1 + K / Q[band] + K * K); + a0[band] = 1 * norm; + a1[band] = -2 * a0[band]; + a2[band] = a0[band]; + b1[band] = 2 * (K * K - 1) * norm; + b2[band] = (1 - K / Q[band] + K * K) * norm; break; - case bq_type_bandpass: - norm = 1 / (1 + K / Q + K * K); - biquad_coefficients->a0 = K / Q * norm; - biquad_coefficients->a1 = 0; - biquad_coefficients->a2 = -biquad_coefficients->a0; - biquad_coefficients->b1 = 2 * (K * K - 1) * norm; - biquad_coefficients->b2 = (1 - K / Q + K * K) * norm; + case EQ_BANDPASS: + norm = 1 / (1 + K / Q[band] + K * K); + a0[band] = K / Q[band] * norm; + a1[band] = 0; + a2[band] = -a0[band]; + b1[band] = 2 * (K * K - 1) * norm; + b2[band] = (1 - K / Q[band] + K * K) * norm; break; - case bq_type_notch: - norm = 1 / (1 + K / Q + K * K); - biquad_coefficients->a0 = (1 + K * K) * norm; - biquad_coefficients->a1 = 2 * (K * K - 1) * norm; - biquad_coefficients->a2 = biquad_coefficients->a0; - biquad_coefficients->b1 = biquad_coefficients->a1; - biquad_coefficients->b2 = (1 - K / Q + K * K) * norm; + case EQ_NOTCH: + norm = 1 / (1 + K / Q[band] + K * K); + a0[band] = (1 + K * K) * norm; + a1[band] = 2 * (K * K - 1) * norm; + a2[band] = a0[band]; + b1[band] = a1[band]; + b2[band] = (1 - K / Q[band] + K * K) * norm; break; - case bq_type_peak: - if (peakGain >= 0) { // boost - norm = 1 / (1 + 1 / Q * K + K * K); - biquad_coefficients->a0 = (1 + V / Q * K + K * K) * norm; - biquad_coefficients->a1 = 2 * (K * K - 1) * norm; - biquad_coefficients->a2 = (1 - V / Q * K + K * K) * norm; - biquad_coefficients->b1 = biquad_coefficients->a1; - biquad_coefficients->b2 = (1 - 1 / Q * K + K * K) * norm; + case EQ_PEAK: + if (peakGainDB[band] >= 0) { // boost + norm = 1 / (1 + 1 / Q[band] * K + K * K); + a0[band] = (1 + V / Q[band] * K + K * K) * norm; + a1[band] = 2 * (K * K - 1) * norm; + a2[band] = (1 - V / Q[band] * K + K * K) * norm; + b1[band] = a1[band]; + b2[band] = (1 - 1 / Q[band] * K + K * K) * norm; } else { // cut - norm = 1 / (1 + V / Q * K + K * K); - biquad_coefficients->a0 = (1 + 1 / Q * K + K * K) * norm; - biquad_coefficients->a1 = 2 * (K * K - 1) * norm; - biquad_coefficients->a2 = (1 - 1 / Q * K + K * K) * norm; - biquad_coefficients->b1 = biquad_coefficients->a1; - biquad_coefficients->b2 = (1 - V / Q * K + K * K) * norm; + norm = 1 / (1 + V / Q[band] * K + K * K); + a0[band] = (1 + 1 / Q[band] * K + K * K) * norm; + a1[band] = 2 * (K * K - 1) * norm; + a2[band] = (1 - 1 / Q[band] * K + K * K) * norm; + b1[band] = a1[band]; + b2[band] = (1 - V / Q[band] * K + K * K) * norm; } break; - case bq_type_lowshelf: - if (peakGain >= 0) { // boost + case EQ_LOWSHELF: + if (peakGainDB[band] >= 0) { // boost norm = 1 / (1 + sqrt(2) * K + K * K); - biquad_coefficients->a0 = (1 + sqrt(2 * V) * K + V * K * K) * norm; - biquad_coefficients->a1 = 2 * (V * K * K - 1) * norm; - biquad_coefficients->a2 = (1 - sqrt(2 * V) * K + V * K * K) * norm; - biquad_coefficients->b1 = 2 * (K * K - 1) * norm; - biquad_coefficients->b2 = (1 - sqrt(2) * K + K * K) * norm; + a0[band] = (1 + sqrt(2 * V) * K + V * K * K) * norm; + a1[band] = 2 * (V * K * K - 1) * norm; + a2[band] = (1 - sqrt(2 * V) * K + V * K * K) * norm; + b1[band] = 2 * (K * K - 1) * norm; + b2[band] = (1 - sqrt(2) * K + K * K) * norm; } else { // cut norm = 1 / (1 + sqrt(2 * V) * K + V * K * K); - biquad_coefficients->a0 = (1 + sqrt(2) * K + K * K) * norm; - biquad_coefficients->a1 = 2 * (K * K - 1) * norm; - biquad_coefficients->a2 = (1 - sqrt(2) * K + K * K) * norm; - biquad_coefficients->b1 = 2 * (V * K * K - 1) * norm; - biquad_coefficients->b2 = (1 - sqrt(2 * V) * K + V * K * K) * norm; + a0[band] = (1 + sqrt(2) * K + K * K) * norm; + a1[band] = 2 * (K * K - 1) * norm; + a2[band] = (1 - sqrt(2) * K + K * K) * norm; + b1[band] = 2 * (V * K * K - 1) * norm; + b2[band] = (1 - sqrt(2 * V) * K + V * K * K) * norm; } break; - case bq_type_highshelf: - if (peakGain >= 0) { // boost + case EQ_HIGHSHELF: + if (peakGainDB[band] >= 0) { // boost norm = 1 / (1 + sqrt(2) * K + K * K); - biquad_coefficients->a0 = (V + sqrt(2 * V) * K + K * K) * norm; - biquad_coefficients->a1 = 2 * (K * K - V) * norm; - biquad_coefficients->a2 = (V - sqrt(2 * V) * K + K * K) * norm; - biquad_coefficients->b1 = 2 * (K * K - 1) * norm; - biquad_coefficients->b2 = (1 - sqrt(2) * K + K * K) * norm; + a0[band] = (V + sqrt(2 * V) * K + K * K) * norm; + a1[band] = 2 * (K * K - V) * norm; + a2[band] = (V - sqrt(2 * V) * K + K * K) * norm; + b1[band] = 2 * (K * K - 1) * norm; + b2[band] = (1 - sqrt(2) * K + K * K) * norm; } else { // cut norm = 1 / (V + sqrt(2 * V) * K + K * K); - biquad_coefficients->a0 = (1 + sqrt(2) * K + K * K) * norm; - biquad_coefficients->a1 = 2 * (K * K - 1) * norm; - biquad_coefficients->a2 = (1 - sqrt(2) * K + K * K) * norm; - biquad_coefficients->b1 = 2 * (K * K - V) * norm; - biquad_coefficients->b2 = (V - sqrt(2 * V) * K + K * K) * norm; + a0[band] = (1 + sqrt(2) * K + K * K) * norm; + a1[band] = 2 * (K * K - 1) * norm; + a2[band] = (1 - sqrt(2) * K + K * K) * norm; + b1[band] = 2 * (K * K - V) * norm; + b2[band] = (V - sqrt(2 * V) * K + K * K) * norm; } break; }