dsp_iir_filter1/iir_filter1.cpp at master · yangrui9501/dsp_iir_filter1 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#include "iir_filter1.h"

void IIRFilter1::init(FilterType filter_type, double _fc, double _T)
{
    u_out = 0.0;
    u_in = 0.0;
    u_in_pre = 0.0;

    fc = _fc;
    T = _T;

    switch (filter_type)
    {
    case TYPE_LPF:
        A1C = 1.0 / (2.0 * PI * fc);
        A2C = 1.0;
        B1C = 0.0;
        B2C = 1.0;
        break;
    case TYPE_HPF:
        A1C = 1.0 / (2.0 * PI * fc);
        A2C = 1.0;
        B1C = 1.0 / (2.0 * PI * fc);
        B2C = 0.0;
        break;
    case TYPE_APD:
        A1C = 1.0 / (2.0 * PI * fc);
        A2C = 1.0;
        B1C = 1.0;
        B2C = 0.0;
        break;
    }
    A1D = 2.0 * A1C + A2C * T;
    A2D = A2C * T - 2.0 * A1C;
    B1D = 2.0 * B1C + B2C * T;
    B2D = B2C * T - 2.0 * B1C;
}

const double& IIRFilter1::update(const double& input_data)
{
    u_in = input_data;

    u_out = 1.0 / A1D * (-A2D * u_out + B1D * u_in + B2D * u_in_pre);
    u_in_pre = u_in;

    return u_out;
}

void IIRFilter1::reset()
{
    u_in_pre = 0.0;
    u_out = 0.0;
}