86 lines
2.2 KiB
C
86 lines
2.2 KiB
C
#include <string.h>
|
|
#include <math.h>
|
|
#include "filter.h"
|
|
#include "psyn.h"
|
|
|
|
void filter_init_rc(struct filter_t *filter, double R, double C)
|
|
{
|
|
memset(filter, 0, sizeof *filter);
|
|
filter->k = 1.0 / (R * C) / _sample_rate;
|
|
filter->r = 1.0;
|
|
}
|
|
|
|
void filter_init_freq(struct filter_t *filter, double freq)
|
|
{
|
|
memset(filter, 0, sizeof *filter);
|
|
filter->k = (2.0 * M_PI * freq) / _sample_rate;
|
|
filter->r = 1.0;
|
|
}
|
|
|
|
void filter_run_lp(struct filter_t *filter, double left, double right)
|
|
{
|
|
filter->v_l += filter->k * (left - filter->last_out_l);
|
|
filter->v_r += filter->k * (right - filter->last_out_r);
|
|
filter->last_out_l = filter->v_l;
|
|
filter->last_out_r = filter->v_r;
|
|
filter->v_l *= filter->r;
|
|
filter->v_r *= filter->r;
|
|
}
|
|
|
|
void filter_run_hp(struct filter_t *filter, double left, double right)
|
|
{
|
|
filter->last_out_l = filter->k * (filter->last_out_l + left - filter->last_in_l);
|
|
filter->last_out_r = filter->k * (filter->last_out_r + right - filter->last_in_r);
|
|
filter->last_in_l = left;
|
|
filter->last_in_r = right;
|
|
}
|
|
|
|
void bw_filter_init_lp(struct bw_filter_t *f, double freq, double resonance)
|
|
{
|
|
double c = 1.0 / tan(M_PI * freq / _sample_rate);
|
|
f->a1 = 1.0 / (1.0 + resonance * c + c * c);
|
|
f->a2 = 2 * f->a1;
|
|
f->a3 = f->a1;
|
|
f->b1 = 2.0 * (1.0 - c * c) * f->a1;
|
|
f->b2 = (1.0 - resonance * c + c * c) * f->a1;
|
|
}
|
|
|
|
void bw_filter_run(struct bw_filter_t *f, double in_l, double in_r, double *out_l, double *out_r)
|
|
{
|
|
*out_l = f->a1 * in_l + f->a2 * f->in1_l + f->a3 * f->in2_l - f->b1 * f->out1_l - f->b2 * f->out2_l;
|
|
*out_r = f->a1 * in_r + f->a2 * f->in1_r + f->a3 * f->in2_r - f->b1 * f->out1_r - f->b2 * f->out2_r;
|
|
|
|
f->in2_l = f->in1_l;
|
|
f->in1_l = in_l;
|
|
f->out2_l = f->out1_l;
|
|
f->out1_l = *out_l;
|
|
|
|
f->in2_r = f->in1_r;
|
|
f->in1_r = in_r;
|
|
f->out2_r = f->out1_r;
|
|
f->out1_r = *out_r;
|
|
}
|
|
|
|
/*
|
|
The filter algo:
|
|
out(n) = a1 * in + a2 * in(n-1) + a3 * in(n-2) - b1*out(n-1) - b2*out(n-2)
|
|
|
|
Lowpass:
|
|
c = 1.0 / tan(pi * f / sample_rate);
|
|
|
|
a1 = 1.0 / ( 1.0 + r * c + c * c);
|
|
a2 = 2* a1;
|
|
a3 = a1;
|
|
b1 = 2.0 * ( 1.0 - c*c) * a1;
|
|
b2 = ( 1.0 - r * c + c * c) * a1;
|
|
|
|
Hipass:
|
|
c = tan(pi * f / sample_rate);
|
|
|
|
a1 = 1.0 / ( 1.0 + r * c + c * c);
|
|
a2 = -2*a1;
|
|
a3 = a1;
|
|
b1 = 2.0 * ( c*c - 1.0) * a1;
|
|
b2 = ( 1.0 - r * c + c * c) * a1;
|
|
*/
|