78 lines
2.3 KiB
C
78 lines
2.3 KiB
C
#include "fx_equalizer.h"
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
|
|
#define FX_EQ_SAMPLE_RATE 48000.0f
|
|
|
|
static const float fx_eq_default_freqs[FX_EQ_NUM_BANDS] =
|
|
{
|
|
31.0f, 62.0f, 125.0f, 250.0f, 500.0f, 1000.0f, 2000.0f, 4000.0f, 8000.0f, 16000.0f
|
|
};
|
|
|
|
void FxEqualizer_Init(FxEqualizerState *state, FxEqualizerParams *params)
|
|
{
|
|
params->enabled = false;
|
|
for (uint32_t i = 0; i < FX_EQ_NUM_BANDS; i++)
|
|
{
|
|
params->bands[i].freq_hz = fx_eq_default_freqs[i];
|
|
params->bands[i].gain_db = 0.0f;
|
|
params->bands[i].q = 1.0f;
|
|
|
|
state->band_state[i].last_freq_hz = -1.0f; /* force le calcul des coefficients au 1er sample */
|
|
state->band_state[i].last_gain_db = 0.0f;
|
|
state->band_state[i].last_q = 0.0f;
|
|
state->x1[i] = state->x2[i] = state->y1[i] = state->y2[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/* Filtre "peaking EQ" (cloche), formule de l'Audio EQ Cookbook (Robert Bristow-Johnson). */
|
|
static void FxEq_ComputeBandCoeffs(FxEqBandState *bs, const FxEqBand *band)
|
|
{
|
|
float A = powf(10.0f, band->gain_db / 40.0f);
|
|
float w0 = 2.0f * (float)M_PI * band->freq_hz / FX_EQ_SAMPLE_RATE;
|
|
float alpha = sinf(w0) / (2.0f * band->q);
|
|
float cosw0 = cosf(w0);
|
|
|
|
float a0 = 1.0f + alpha / A;
|
|
bs->b0 = (1.0f + alpha * A) / a0;
|
|
bs->b1 = (-2.0f * cosw0) / a0;
|
|
bs->b2 = (1.0f - alpha * A) / a0;
|
|
bs->a1 = (-2.0f * cosw0) / a0;
|
|
bs->a2 = (1.0f - alpha / A) / a0;
|
|
|
|
bs->last_freq_hz = band->freq_hz;
|
|
bs->last_gain_db = band->gain_db;
|
|
bs->last_q = band->q;
|
|
}
|
|
|
|
float FxEqualizer_Process(FxEqualizerState *state, const FxEqualizerParams *params, float input)
|
|
{
|
|
if (!params->enabled)
|
|
{
|
|
return input;
|
|
}
|
|
|
|
float y = input;
|
|
for (uint32_t i = 0; i < FX_EQ_NUM_BANDS; i++)
|
|
{
|
|
const FxEqBand *band = ¶ms->bands[i];
|
|
FxEqBandState *bs = &state->band_state[i];
|
|
|
|
if (band->freq_hz != bs->last_freq_hz || band->gain_db != bs->last_gain_db || band->q != bs->last_q)
|
|
{
|
|
FxEq_ComputeBandCoeffs(bs, band);
|
|
}
|
|
|
|
float x = y;
|
|
y = bs->b0 * x + bs->b1 * state->x1[i] + bs->b2 * state->x2[i]
|
|
- bs->a1 * state->y1[i] - bs->a2 * state->y2[i];
|
|
|
|
state->x2[i] = state->x1[i];
|
|
state->x1[i] = x;
|
|
state->y2[i] = state->y1[i];
|
|
state->y1[i] = y;
|
|
}
|
|
|
|
return y;
|
|
}
|