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.
 
 
psyn/filter.c

85 lines
2.2 KiB

#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;
*/