Implement Parametric EQ filter
This commit is contained in:
parent
ccbd21e64f
commit
463ff5869e
3 changed files with 476 additions and 0 deletions
437
audio/audio_filter_param_eq.c
Normal file
437
audio/audio_filter_param_eq.c
Normal file
|
|
@ -0,0 +1,437 @@
|
|||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "audio.h"
|
||||
#include "audio_internal.h"
|
||||
#include "audio_filter_param_eq.h"
|
||||
|
||||
#ifdef FIXEDPOINT
|
||||
# define PRECISION int32_t
|
||||
#elif defined(LOW_PRECISION)
|
||||
# define PRECISION float
|
||||
#else
|
||||
# define PRECISION double
|
||||
#endif
|
||||
|
||||
typedef PRECISION biquad_type_t;
|
||||
|
||||
typedef struct _BiQuad {
|
||||
biquad_type_t b0, b1, b2, a1, a2; // coefficients
|
||||
biquad_type_t x1, x2; // history
|
||||
#ifdef SECOND_ORDER_FILTER
|
||||
biquad_type_t y1, y2; // history for second stage
|
||||
#endif
|
||||
} BiQuad;
|
||||
|
||||
typedef struct _FilterParamEQContext {
|
||||
EQBand *bands;
|
||||
uint8_t numBands;
|
||||
BiQuad **elements; // [channel][band]
|
||||
} FilterParamEQContext;
|
||||
|
||||
#ifdef SECOND_ORDER_FILTER
|
||||
static inline int16_t biquad_apply(BiQuad *element, int16_t sample) {
|
||||
#ifdef FIXEDPOINT
|
||||
int32_t result;
|
||||
|
||||
result = \
|
||||
((element->b0 * sample) >> 15) \
|
||||
+ ((element->b1 * element->x1) >> 15) \
|
||||
+ ((element->b2 * element->x2) >> 15) \
|
||||
- ((element->a1 * element->y1) >> 15) \
|
||||
- ((element->a2 * element->y2) >> 15);
|
||||
|
||||
// clip output
|
||||
int16_t out = result;
|
||||
|
||||
if (result > INT16_MAX) {
|
||||
out = INT16_MAX;
|
||||
}
|
||||
if (result < INT16_MIN) {
|
||||
out = INT16_MIN;
|
||||
}
|
||||
|
||||
/* shift x1 to x2, sample to x1 */
|
||||
element->x2 = element->x1;
|
||||
element->x1 = sample;
|
||||
|
||||
/* shift y1 to y2, result to y1 */
|
||||
element->y2 = element->y1;
|
||||
element->y1 = out;
|
||||
|
||||
return out;
|
||||
#else
|
||||
biquad_type_t result;
|
||||
|
||||
/* compute result */
|
||||
result = \
|
||||
element->b0 * (biquad_type_t)sample \
|
||||
+ element->b1 * element->x1 \
|
||||
+ element->b2 * element->x2 \
|
||||
- element->a1 * element->y1 \
|
||||
- element->a2 * element->y2;
|
||||
|
||||
/* shift x1 to x2, sample to x1 */
|
||||
element->x2 = element->x1;
|
||||
element->x1 = (double)sample;
|
||||
|
||||
/* shift y1 to y2, result to y1 */
|
||||
element->y2 = element->y1;
|
||||
element->y1 = result;
|
||||
|
||||
result = round(result);
|
||||
if (result > INT16_MAX) {
|
||||
return INT16_MAX - 1;
|
||||
}
|
||||
if (result < INT16_MIN) {
|
||||
return INT16_MIN + 1;
|
||||
}
|
||||
return (int16_t)result;
|
||||
#endif
|
||||
}
|
||||
|
||||
static BiQuad biquad_initialize(EQBand band, uint32_t sampleRate) {
|
||||
BiQuad b;
|
||||
double A, omega, sn, cs, alpha, beta;
|
||||
double a0, a1, a2, b0, b1, b2;
|
||||
|
||||
double dbGain = band.gain;
|
||||
double freq = band.frequency;
|
||||
double bandwidth = band.Q;
|
||||
|
||||
/* setup variables */
|
||||
A = pow(10, dbGain / 40);
|
||||
omega = 2 * M_PI * freq / (double)sampleRate;
|
||||
sn = sin(omega);
|
||||
cs = cos(omega);
|
||||
alpha = sn * sinh(M_LN2 / 2 * bandwidth * omega / sn);
|
||||
beta = sqrt(A + A);
|
||||
|
||||
switch (band.type) {
|
||||
case EQTypeLowPass:
|
||||
b0 = (1 - cs) /2;
|
||||
b1 = 1 - cs;
|
||||
b2 = (1 - cs) /2;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
break;
|
||||
case EQTypeHighPass:
|
||||
b0 = (1 + cs) /2;
|
||||
b1 = -(1 + cs);
|
||||
b2 = (1 + cs) /2;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
break;
|
||||
case EQTypeBandPass:
|
||||
b0 = alpha;
|
||||
b1 = 0;
|
||||
b2 = -alpha;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
break;
|
||||
case EQTypeNotch:
|
||||
b0 = 1;
|
||||
b1 = -2 * cs;
|
||||
b2 = 1;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
break;
|
||||
case EQTypePeak:
|
||||
b0 = 1 + (alpha * A);
|
||||
b1 = -2 * cs;
|
||||
b2 = 1 - (alpha * A);
|
||||
a0 = 1 + (alpha /A);
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - (alpha /A);
|
||||
break;
|
||||
case EQTypeLowShelf:
|
||||
b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
|
||||
b1 = 2 * A * ((A - 1) - (A + 1) * cs);
|
||||
b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
|
||||
a0 = (A + 1) + (A - 1) * cs + beta * sn;
|
||||
a1 = -2 * ((A - 1) + (A + 1) * cs);
|
||||
a2 = (A + 1) + (A - 1) * cs - beta * sn;
|
||||
break;
|
||||
case EQTypeHighShelf:
|
||||
b0 = A * ((A + 1) + (A - 1) * cs + beta * sn);
|
||||
b1 = -2 * A * ((A - 1) + (A + 1) * cs);
|
||||
b2 = A * ((A + 1) + (A - 1) * cs - beta * sn);
|
||||
a0 = (A + 1) - (A - 1) * cs + beta * sn;
|
||||
a1 = 2 * ((A - 1) - (A + 1) * cs);
|
||||
a2 = (A + 1) - (A - 1) * cs - beta * sn;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* precompute the coefficients */
|
||||
#ifdef FIXEDPOINT
|
||||
b.b0 = b0 / a0 * INT16_MAX;
|
||||
b.b1 = b1 / a0 * INT16_MAX;
|
||||
b.b2 = b2 / a0 * INT16_MAX;
|
||||
b.a1 = a1 / a0 * INT16_MAX;
|
||||
b.a2 = a2 / a0 * INT16_MAX;
|
||||
#else
|
||||
b.b0 = b0 / a0;
|
||||
b.b1 = b1 / a0;
|
||||
b.b2 = b2 / a0;
|
||||
b.a1 = a1 / a0;
|
||||
b.a2 = a2 / a0;
|
||||
#endif
|
||||
|
||||
/* zero initial samples */
|
||||
b.x1 = b.x2 = 0;
|
||||
b.y1 = b.y2 = 0;
|
||||
|
||||
return b;
|
||||
}
|
||||
#else
|
||||
static BiQuad biquad_initialize(EQBand band, uint32_t sampleRate) {
|
||||
BiQuad b;
|
||||
double b0, b1, b2, a1, a2;
|
||||
double Fc = band.frequency / sampleRate;
|
||||
|
||||
double norm;
|
||||
double V = pow(10, fabs(band.gain) / 20.0);
|
||||
double K = tan(M_PI * Fc);
|
||||
|
||||
switch (band.type) {
|
||||
case EQTypeLowPass:
|
||||
norm = 1 / (1 + K / band.Q + K * K);
|
||||
b0 = K * K * norm;
|
||||
b1 = 2 * b0;
|
||||
b2 = b0;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - K / band.Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case EQTypeHighPass:
|
||||
norm = 1 / (1 + K / band.Q + K * K);
|
||||
b0 = 1 * norm;
|
||||
b1 = -2 * b0;
|
||||
b2 = b0;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - K / band.Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case EQTypeBandPass:
|
||||
norm = 1 / (1 + K / band.Q + K * K);
|
||||
b0 = K / band.Q * norm;
|
||||
b1 = 0;
|
||||
b2 = -b0;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - K / band.Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case EQTypeNotch:
|
||||
norm = 1 / (1 + K / band.Q + K * K);
|
||||
b0 = (1 + K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = b0;
|
||||
a1 = b1;
|
||||
a2 = (1 - K / band.Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case EQTypePeak:
|
||||
if (band.gain >= 0) { // boost
|
||||
norm = 1 / (1 + 1/band.Q * K + K * K);
|
||||
b0 = (1 + V/band.Q * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - V/band.Q * K + K * K) * norm;
|
||||
a1 = b1;
|
||||
a2 = (1 - 1/band.Q * K + K * K) * norm;
|
||||
}
|
||||
else { // cut
|
||||
norm = 1 / (1 + V/band.Q * K + K * K);
|
||||
b0 = (1 + 1/band.Q * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - 1/band.Q * K + K * K) * norm;
|
||||
a1 = b1;
|
||||
a2 = (1 - V/band.Q * K + K * K) * norm;
|
||||
}
|
||||
break;
|
||||
case EQTypeLowShelf:
|
||||
if (band.gain >= 0) { // boost
|
||||
norm = 1 / (1 + sqrt(2) * K + K * K);
|
||||
b0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
|
||||
b1 = 2 * (V * K * K - 1) * norm;
|
||||
b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
}
|
||||
else { // cut
|
||||
norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
|
||||
b0 = (1 + sqrt(2) * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
a1 = 2 * (V * K * K - 1) * norm;
|
||||
a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
|
||||
}
|
||||
break;
|
||||
case EQTypeHighShelf:
|
||||
if (band.gain >= 0) { // boost
|
||||
norm = 1 / (1 + sqrt(2) * K + K * K);
|
||||
b0 = (V + sqrt(2*V) * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - V) * norm;
|
||||
b2 = (V - sqrt(2*V) * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
}
|
||||
else { // cut
|
||||
norm = 1 / (V + sqrt(2*V) * K + K * K);
|
||||
b0 = (1 + sqrt(2) * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - V) * norm;
|
||||
a2 = (V - sqrt(2*V) * K + K * K) * norm;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef FIXEDPOINT
|
||||
b.b0 = b0 * INT16_MAX;
|
||||
b.b1 = b1 * INT16_MAX;
|
||||
b.b2 = b2 * INT16_MAX;
|
||||
b.a1 = a1 * INT16_MAX;
|
||||
b.a2 = a2 * INT16_MAX;
|
||||
#else
|
||||
b.b0 = b0;
|
||||
b.b1 = b1;
|
||||
b.b2 = b2;
|
||||
b.a1 = a1;
|
||||
b.a2 = a2;
|
||||
#endif
|
||||
|
||||
return b;
|
||||
}
|
||||
|
||||
static inline int16_t biquad_apply(BiQuad *element, int16_t sample) {
|
||||
#ifdef FIXEDPOINT
|
||||
int32_t result = ((sample * element->b0) >> 15) + element->x1;
|
||||
|
||||
// clip output
|
||||
int16_t out = result;
|
||||
|
||||
if (result > INT16_MAX) {
|
||||
out = INT16_MAX;
|
||||
}
|
||||
if (result < INT16_MIN) {
|
||||
out = INT16_MIN;
|
||||
}
|
||||
|
||||
// save delayed state for recursion
|
||||
element->x1 = ((sample * element->b1) >> 15) + element->x2 - ((element->a1 * out) >> 15);
|
||||
element->x2 = ((sample * element->b2) >> 15) - ((element->a2 * out) >> 15);
|
||||
|
||||
return out;
|
||||
#else
|
||||
// Calculate filter
|
||||
biquad_type_t result = sample * element->b0 + element->x1;
|
||||
element->x1 = sample * element->b1 + element->x2 - element->a1 * result;
|
||||
element->x2 = sample * element->b2 - element->a2 * result;
|
||||
|
||||
// Round and clip
|
||||
result = round(result);
|
||||
if (result > INT16_MAX) {
|
||||
return INT16_MAX - 1;
|
||||
}
|
||||
if (result < INT16_MIN) {
|
||||
return INT16_MIN + 1;
|
||||
}
|
||||
return (int16_t)result;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
AudioPipelineStatus filter_param_eq_push(AudioPipelineElement *self, AudioBuffer *buffer) {
|
||||
FilterParamEQContext *context = (FilterParamEQContext *)self->ctx;
|
||||
int16_t *buf = (int16_t *)buffer->data;
|
||||
|
||||
for (uint32_t s = 0; s < buffer->buf_size / 2; s+= self->channels) {
|
||||
for (uint8_t ch = 0; ch < self->channels; ch++) {
|
||||
int16_t result = buf[s + ch];
|
||||
for (uint8_t band = 0; band < context->numBands; band++) {
|
||||
result = biquad_apply(&context->elements[ch][band], result);
|
||||
}
|
||||
buf[s + ch] = result;
|
||||
}
|
||||
}
|
||||
|
||||
// run next element of the pipeline
|
||||
return self->next->push(self->next, buffer);
|
||||
}
|
||||
|
||||
AudioPipelineStatus filter_param_eq_link(AudioPipelineElement *self, AudioPipelineElement *source) {
|
||||
FilterParamEQContext *context = (FilterParamEQContext *)self->ctx;
|
||||
|
||||
if (source->bits_per_sample != 16) {
|
||||
fprintf(stderr, "ERROR: Parametric EQ only works on 16 bits/sample\n");
|
||||
return PipelineError;
|
||||
}
|
||||
self->sample_rate = source->sample_rate;
|
||||
self->bits_per_sample = source->bits_per_sample;
|
||||
self->channels = source->channels;
|
||||
|
||||
// Initialize Coefficients and zero out delay line
|
||||
if (context->elements != NULL) {
|
||||
free(context->elements);
|
||||
context->elements = NULL;
|
||||
}
|
||||
context->elements = calloc(self->channels, sizeof(BiQuad *));
|
||||
for (uint8_t ch = 0; ch < self->channels; ch++) {
|
||||
context->elements[ch] = calloc(context->numBands, sizeof(BiQuad));
|
||||
for (uint8_t band = 0; band < context->numBands; band++) {
|
||||
context->elements[ch][band] = biquad_initialize(context->bands[band], self->sample_rate);
|
||||
}
|
||||
}
|
||||
|
||||
// link next element
|
||||
source->next = self;
|
||||
if (self->next != NULL) {
|
||||
// relinking after samplerate change
|
||||
return self->next->link(self->next, self);
|
||||
}
|
||||
return PipelineStopped;
|
||||
}
|
||||
|
||||
char *filter_param_eq_describe(AudioPipelineElement *self) {
|
||||
return "biquad parametric EQ";
|
||||
}
|
||||
|
||||
void filter_param_eq_destroy(AudioPipelineElement *self) {
|
||||
FilterParamEQContext *context = (FilterParamEQContext *)self->ctx;
|
||||
free(context);
|
||||
free(self);
|
||||
}
|
||||
|
||||
AudioPipelineElement *audio_filter_param_eq(EQBand *bands, int numBands, float prescale) {
|
||||
AudioPipelineElement *self = calloc(1, sizeof(AudioPipelineElement));
|
||||
FilterParamEQContext *context = calloc(1, sizeof(FilterParamEQContext));
|
||||
|
||||
context->bands = malloc(numBands * sizeof(EQBand));
|
||||
memcpy(context->bands, bands, numBands * sizeof(EQBand));
|
||||
for (uint8_t band = 0; band < numBands; band++) {
|
||||
context->bands[band].gain += prescale;
|
||||
}
|
||||
context->numBands = numBands;
|
||||
context->elements = NULL;
|
||||
|
||||
self->ctx = context;
|
||||
self->describe = filter_param_eq_describe;
|
||||
self->start = filter_start_nop;
|
||||
self->reset = filter_reset_nop;
|
||||
self->stop = filter_stop_nop;
|
||||
self->push = filter_param_eq_push;
|
||||
self->link = filter_param_eq_link;
|
||||
self->destroy = filter_param_eq_destroy;
|
||||
self->type = AudioElementFilter;
|
||||
|
||||
return self;
|
||||
}
|
||||
39
audio/include/audio_filter_param_eq.h
Normal file
39
audio/include/audio_filter_param_eq.h
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
#ifndef AUDIOPIPELINE_AUDIO_FILTER_PARAM_EQ_H__INCLUDED
|
||||
#define AUDIOPIPELINE_AUDIO_FILTER_PARAM_EQ_H__INCLUDED
|
||||
|
||||
#include "audio.h"
|
||||
|
||||
typedef enum _EQBandType {
|
||||
EQTypeLowPass, /** simple low pass filter */
|
||||
EQTypeLowShelf, /** low shelf filter, basically all values before the center frequency get modified by the gain*/
|
||||
EQTypeBandPass, /** only frequencies around the center frequency will pass through */
|
||||
EQTypePeak, /** amplify frequencies around the center frequency */
|
||||
EQTypeNotch, /** duck frequencies around the center frequency */
|
||||
EQTypeHighPass, /** simple high pass filter */
|
||||
EQTypeHighShelf /** high shelf, like the low shelf filter but affects all frequencies after the center */
|
||||
} EQBandType;
|
||||
|
||||
typedef struct _EQBand {
|
||||
EQBandType type; /** type of filter for this band */
|
||||
float frequency; /** EQ band center frequency */
|
||||
float Q; /** Filter bandwidth, not used by shelf filters */
|
||||
float gain; /** Only used for Peak, Notch and Shelf filters*/
|
||||
} EQBand;
|
||||
|
||||
/**
|
||||
* Creates a filter element which applies a parametric EQ to the audio samples
|
||||
*
|
||||
* @param bands Array of EQ bands to apply, start with low shelf and end on high shelf if possible
|
||||
* @param numBands number of bands in the array
|
||||
* @param prescale prescale all bands by this value
|
||||
*
|
||||
* @returns Initialized `AudioPipelineElement` that can be used in call to `audio_pipeline_assemble`
|
||||
*/
|
||||
AudioPipelineElement *audio_filter_param_eq(EQBand *bands, int numBands, float prescale);
|
||||
|
||||
#undef SECOND_ORDER_FILTER /* use a second biquad stage for steeper cutoff frequencies, needs more processing power */
|
||||
#undef FIXEDPOINT /* run in fixed point mode, floats are used only on initialization, filter unstable below 300 Hz! */
|
||||
#define LOW_PRECISION /* run filter on float32 instead of double precision */
|
||||
|
||||
#endif /* AUDIOPIPELINE_AUDIO_FILTER_PARAM_EQ_H__INCLUDED */
|
||||
|
||||
Loading…
Reference in a new issue