You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
189 lines
7.0 KiB
189 lines
7.0 KiB
3 years ago
|
#ifdef SGTL5000_AUDIO_ENHANCE
|
||
|
|
|
||
|
|
#include <Arduino.h>
|
||
|
|
#include <Audio.h>
|
||
|
|
|
||
|
|
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
|
||
|
|
};
|
||
|
|
|
||
|
|
#define NUM_GRAPHIC_EQ_BANDS 7
|
||
|
|
|
||
|
|
biquad_coefficients_t biquad_coefficients[NUM_GRAPHIC_EQ_BANDS];
|
||
|
|
biquad_params_t biquad_params[NUM_GRAPHIC_EQ_BANDS];
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
void set_sgtl5000_7band_eq(uint8_t band, uint8_t filter_type, float32_t Fc, float32_t Q)
|
||
|
|
{
|
||
|
|
biquad_params[band].filter_type = filter_type;
|
||
|
|
biquad_params[band].Fc = Fc;
|
||
|
|
biquad_params[band].Q = Q;
|
||
|
|
|
||
|
|
calcBiquadCoefficients(&biquad_coefficients[band], filter_type, Fc, Q, 0.0);
|
||
|
|
|
||
|
|
//sgtl5000_1.eqFilter(band, biquad_coefficients);
|
||
|
|
}
|
||
|
|
|
||
|
|
void set_sgtl5000_7band_eq_gain(uint8_t band, float32_t peakGainDB)
|
||
|
|
{
|
||
|
|
calcBiquadCoefficients(&biquad_coefficients[band], biquad_params[band].filter_type, biquad_params[band].Fc, biquad_params[band].Q, peakGainDB);
|
||
|
|
//sgtl5000_1.eqFilter(band, biquad_coefficients);
|
||
|
|
}
|
||
|
|
|
||
|
|
void setup_sgtl5000_graphic_7band_eq(void)
|
||
|
|
{
|
||
|
|
//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);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Taken from https://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
|
||
|
|
//
|
||
|
|
// Biquad.h
|
||
|
|
//
|
||
|
|
// Created by Nigel Redmon on 11/24/12
|
||
|
|
// EarLevel Engineering: earlevel.com
|
||
|
|
// Copyright 2012 Nigel Redmon
|
||
|
|
//
|
||
|
|
// For a complete explanation of the Biquad code:
|
||
|
|
// http://www.earlevel.com/main/2012/11/25/biquad-c-source-code/
|
||
|
|
//
|
||
|
|
// License:
|
||
|
|
//
|
||
|
|
// This source code is provided as is, without warranty.
|
||
|
|
// You may copy and distribute verbatim copies of this document.
|
||
|
|
// 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)
|
||
|
|
{
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case bq_type_lowshelf:
|
||
|
|
if (peakGain >= 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;
|
||
|
|
}
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case bq_type_highshelf:
|
||
|
|
if (peakGain >= 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;
|
||
|
|
}
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
#endif
|