48 lines
998 B
C
48 lines
998 B
C
/*
|
|
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
|
*/
|
|
|
|
|
|
#include <math.h>
|
|
#include <filter.h>
|
|
|
|
void lpf2_init(lpf2_t *lpf, double freq) {
|
|
lpf->x0 = 0.0;
|
|
lpf->x1 = 0.0;
|
|
|
|
double order = 1.5;
|
|
double n = 1 / sqrt(pow(2, 1.0 / order) - 1);
|
|
lpf->rc = 1 / (2 * n * M_PI * freq);
|
|
}
|
|
|
|
|
|
double lpf2_apply(lpf2_t *lpf, double x2, double dt) {
|
|
|
|
double k = dt / (lpf->rc + dt);
|
|
|
|
lpf->x1 = lpf->x1 + k * (x2 - lpf->x1);
|
|
lpf->x0 = lpf->x0 + k * (lpf->x1 - lpf->x0);
|
|
return lpf->x0;
|
|
}
|
|
|
|
|
|
void lpf3_init(lpf3_t *lpf, double freq) {
|
|
lpf->x1 = 0.0;
|
|
lpf->x2 = 0.0;
|
|
lpf->x3 = 0.0;
|
|
|
|
double order = 2.0;
|
|
double c = 1.0 / sqrt(powf(2.0, 1.0 / order) - 1.0);
|
|
lpf->rc = 1.0 / (2.0 * c * M_PI * freq);
|
|
}
|
|
|
|
double lpf3_apply(lpf3_t *lpf, double x0, double dt) {
|
|
|
|
double k = dt / (lpf->rc + dt);
|
|
|
|
lpf->x1 = lpf->x1 + k * (x0 - lpf->x1);
|
|
lpf->x2 = lpf->x2 + k * (lpf->x1 - lpf->x2);
|
|
lpf->x3 = lpf->x3 + k * (lpf->x2 - lpf->x3);
|
|
return lpf->x3;
|
|
}
|