mirror of
https://github.com/aicodix/dsp.git
synced 2026-04-27 14:30:36 +00:00
109 lines
2.3 KiB
C++
109 lines
2.3 KiB
C++
/*
|
|
Digital biquad filter
|
|
|
|
Copyright 2019 Ahmet Inan <inan@aicodix.de>
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "const.hh"
|
|
#include "unit_circle.hh"
|
|
|
|
namespace DSP {
|
|
|
|
template <typename TYPE, typename VALUE>
|
|
class Biquad
|
|
{
|
|
TYPE x1, x2, y1, y2;
|
|
VALUE b0a0, b1a0, b2a0, a1a0, a2a0;
|
|
public:
|
|
constexpr Biquad() : x1(0), x2(0), y1(0), y2(0),
|
|
b0a0(1), b1a0(0), b2a0(0), a1a0(0), a2a0(0)
|
|
{
|
|
}
|
|
void lowpass(int n, int N, VALUE Q = Const<VALUE>::InvSqrtTwo())
|
|
{
|
|
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * Q),
|
|
cn = UnitCircle<VALUE>::cos(n, N),
|
|
b0 = (VALUE(1) - cn) / VALUE(2),
|
|
b1 = VALUE(1) - cn,
|
|
b2 = (VALUE(1) - cn) / VALUE(2),
|
|
a0 = VALUE(1) + alpha,
|
|
a1 = -VALUE(2) * cn,
|
|
a2 = VALUE(1) - alpha;
|
|
b0a0 = b0 / a0;
|
|
b1a0 = b1 / a0;
|
|
b2a0 = b2 / a0;
|
|
a1a0 = a1 / a0;
|
|
a2a0 = a2 / a0;
|
|
}
|
|
void highpass(int n, int N, VALUE Q = Const<VALUE>::InvSqrtTwo())
|
|
{
|
|
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * Q),
|
|
cn = UnitCircle<VALUE>::cos(n, N),
|
|
b0 = (VALUE(1) + cn) / VALUE(2),
|
|
b1 = -(VALUE(1) + cn),
|
|
b2 = (VALUE(1) + cn) / VALUE(2),
|
|
a0 = VALUE(1) + alpha,
|
|
a1 = -VALUE(2) * cn,
|
|
a2 = VALUE(1) - alpha;
|
|
b0a0 = b0 / a0;
|
|
b1a0 = b1 / a0;
|
|
b2a0 = b2 / a0;
|
|
a1a0 = a1 / a0;
|
|
a2a0 = a2 / a0;
|
|
}
|
|
void notch(int n, int N, VALUE Q)
|
|
{
|
|
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * Q),
|
|
cn = UnitCircle<VALUE>::cos(n, N),
|
|
b0 = VALUE(1),
|
|
b1 = -VALUE(2) * cn,
|
|
b2 = VALUE(1),
|
|
a0 = VALUE(1) + alpha,
|
|
a1 = -VALUE(2) * cn,
|
|
a2 = VALUE(1) - alpha;
|
|
b0a0 = b0 / a0;
|
|
b1a0 = b1 / a0;
|
|
b2a0 = b2 / a0;
|
|
a1a0 = a1 / a0;
|
|
a2a0 = a2 / a0;
|
|
}
|
|
TYPE operator()(TYPE x0)
|
|
{
|
|
TYPE y0 = b0a0*x0 + b1a0*x1 + b2a0*x2
|
|
- a1a0*y1 - a2a0*y2;
|
|
x2 = x1; x1 = x0;
|
|
y2 = y1; y1 = y0;
|
|
return y0;
|
|
}
|
|
};
|
|
|
|
template <typename TYPE, typename VALUE, int NUM = 1>
|
|
class BiquadCascade
|
|
{
|
|
static const int ORDER = 2 * NUM;
|
|
Biquad<TYPE, VALUE> cascade[NUM];
|
|
public:
|
|
void lowpass(int n, int N)
|
|
{
|
|
for (int i = 0; i < NUM; ++i)
|
|
cascade[i].lowpass(n, N, VALUE(1) / (VALUE(2) *
|
|
UnitCircle<VALUE>::cos(2*i+1, 4*ORDER)));
|
|
}
|
|
void highpass(int n, int N)
|
|
{
|
|
for (int i = 0; i < NUM; ++i)
|
|
cascade[i].highpass(n, N, VALUE(1) / (VALUE(2) *
|
|
UnitCircle<VALUE>::cos(2*i+1, 4*ORDER)));
|
|
}
|
|
TYPE operator()(TYPE input)
|
|
{
|
|
for (int i = 0; i < NUM; ++i)
|
|
input = cascade[i](input);
|
|
return input;
|
|
}
|
|
};
|
|
|
|
}
|
|
|