Adding a working implementation of a Biquad filter adapted for use with the fixed-point CPU of the ESP8266 (#370)
This commit is contained in:
1 parent
6ecb6afe30
commit
714e8cf594
2 files changed
+325
No files matched your search
@@ -0,0 +1,245 @@
|
||||
/*
|
||||
AudioOutputFilterBiquad
|
||||
Implements a Biquad filter
|
||||
|
||||
Copyright (C) 2012 Nigel Redmon
|
||||
Copyright (C) 2021 William Bérubé
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "AudioOutputFilterBiquad.h"
|
||||
|
||||
AudioOutputFilterBiquad::AudioOutputFilterBiquad(AudioOutput *sink)
|
||||
{
|
||||
this->sink = sink;
|
||||
|
||||
type = bq_type_lowpass;
|
||||
a0 = 1.0;
|
||||
a1 = a2 = b1 = b2 = 0.0;
|
||||
Fc = 0.50;
|
||||
Q = 0.707;
|
||||
peakGain = 0.0;
|
||||
z1 = z2 = 0.0;
|
||||
}
|
||||
|
||||
AudioOutputFilterBiquad::AudioOutputFilterBiquad(int type, float Fc, float Q, float peakGain, AudioOutput *sink)
|
||||
{
|
||||
this->sink = sink;
|
||||
|
||||
SetBiquad(type, Fc, Q, peakGain);
|
||||
z1 = z2 = 0.0;
|
||||
}
|
||||
|
||||
AudioOutputFilterBiquad::~AudioOutputFilterBiquad() {}
|
||||
|
||||
bool AudioOutputFilterBiquad::SetRate(int hz)
|
||||
{
|
||||
return sink->SetRate(hz);
|
||||
}
|
||||
|
||||
bool AudioOutputFilterBiquad::SetBitsPerSample(int bits)
|
||||
{
|
||||
return sink->SetBitsPerSample(bits);
|
||||
}
|
||||
|
||||
bool AudioOutputFilterBiquad::SetChannels(int channels)
|
||||
{
|
||||
return sink->SetChannels(channels);
|
||||
}
|
||||
|
||||
bool AudioOutputFilterBiquad::SetGain(float gain)
|
||||
{
|
||||
return sink->SetGain(gain);
|
||||
}
|
||||
|
||||
void AudioOutputFilterBiquad::SetType(int type)
|
||||
{
|
||||
this->type = type;
|
||||
CalcBiquad();
|
||||
}
|
||||
|
||||
void AudioOutputFilterBiquad::SetFc(float Fc)
|
||||
{
|
||||
this->Fc = Fc;
|
||||
CalcBiquad();
|
||||
}
|
||||
|
||||
void AudioOutputFilterBiquad::SetQ(float Q)
|
||||
{
|
||||
this->Q = Q;
|
||||
CalcBiquad();
|
||||
}
|
||||
|
||||
void AudioOutputFilterBiquad::SetPeakGain(float peakGain)
|
||||
{
|
||||
this->peakGain = peakGain;
|
||||
CalcBiquad();
|
||||
}
|
||||
|
||||
void AudioOutputFilterBiquad::SetBiquad(int type, float Fc, float Q, float peakGain)
|
||||
{
|
||||
this->type = type;
|
||||
this->Fc = Fc;
|
||||
this->Q = Q;
|
||||
this->peakGain = peakGain;
|
||||
CalcBiquad();
|
||||
}
|
||||
|
||||
void AudioOutputFilterBiquad::CalcBiquad()
|
||||
{
|
||||
float norm;
|
||||
float V = pow(10, fabs(peakGain) / 20.0);
|
||||
float K = tan(M_PI * Fc);
|
||||
|
||||
switch (this->type) {
|
||||
case bq_type_lowpass:
|
||||
norm = 1 / (1 + K / Q + K * K);
|
||||
a0 = K * K * norm;
|
||||
a1 = 2 * a0;
|
||||
a2 = a0;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - K / Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case bq_type_highpass:
|
||||
norm = 1 / (1 + K / Q + K * K);
|
||||
a0 = 1 * norm;
|
||||
a1 = -2 * a0;
|
||||
a2 = a0;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - K / Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case bq_type_bandpass:
|
||||
norm = 1 / (1 + K / Q + K * K);
|
||||
a0 = K / Q * norm;
|
||||
a1 = 0;
|
||||
a2 = -a0;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - K / Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case bq_type_notch:
|
||||
norm = 1 / (1 + K / Q + K * K);
|
||||
a0 = (1 + K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = a0;
|
||||
b1 = a1;
|
||||
b2 = (1 - K / Q + K * K) * norm;
|
||||
break;
|
||||
|
||||
case bq_type_peak:
|
||||
if (peakGain >= 0) { // boost
|
||||
norm = 1 / (1 + 1/Q * K + K * K);
|
||||
a0 = (1 + V/Q * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - V/Q * K + K * K) * norm;
|
||||
b1 = a1;
|
||||
b2 = (1 - 1/Q * K + K * K) * norm;
|
||||
} else { // cut
|
||||
norm = 1 / (1 + V/Q * K + K * K);
|
||||
a0 = (1 + 1/Q * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - 1/Q * K + K * K) * norm;
|
||||
b1 = a1;
|
||||
b2 = (1 - V/Q * K + K * K) * norm;
|
||||
}
|
||||
break;
|
||||
|
||||
case bq_type_lowshelf:
|
||||
if (peakGain >= 0) { // boost
|
||||
norm = 1 / (1 + sqrt(2) * K + K * K);
|
||||
a0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
|
||||
a1 = 2 * (V * K * K - 1) * norm;
|
||||
a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
}
|
||||
else { // cut
|
||||
norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
|
||||
a0 = (1 + sqrt(2) * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
b1 = 2 * (V * K * K - 1) * norm;
|
||||
b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
|
||||
}
|
||||
break;
|
||||
|
||||
case bq_type_highshelf:
|
||||
if (peakGain >= 0) { // boost
|
||||
norm = 1 / (1 + sqrt(2) * K + K * K);
|
||||
a0 = (V + sqrt(2*V) * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - V) * norm;
|
||||
a2 = (V - sqrt(2*V) * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - 1) * norm;
|
||||
b2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
}
|
||||
else { // cut
|
||||
norm = 1 / (V + sqrt(2*V) * K + K * K);
|
||||
a0 = (1 + sqrt(2) * K + K * K) * norm;
|
||||
a1 = 2 * (K * K - 1) * norm;
|
||||
a2 = (1 - sqrt(2) * K + K * K) * norm;
|
||||
b1 = 2 * (K * K - V) * norm;
|
||||
b2 = (V - sqrt(2*V) * K + K * K) * norm;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
i_a0 = a0 * BQ_DECAL;
|
||||
i_a1 = a1 * BQ_DECAL;
|
||||
i_a2 = a2 * BQ_DECAL;
|
||||
|
||||
i_b1 = b1 * BQ_DECAL;
|
||||
i_b2 = b2 * BQ_DECAL;
|
||||
|
||||
i_lz1 = i_rz1 = z1 * BQ_DECAL;
|
||||
i_lz2 = i_rz2 = z2 * BQ_DECAL;
|
||||
|
||||
i_Fc = Fc * BQ_DECAL;
|
||||
i_Q = Q * BQ_DECAL;
|
||||
i_peakGain = peakGain * BQ_DECAL;
|
||||
}
|
||||
|
||||
bool AudioOutputFilterBiquad::begin()
|
||||
{
|
||||
return sink->begin();
|
||||
}
|
||||
|
||||
bool AudioOutputFilterBiquad::ConsumeSample(int16_t sample[2])
|
||||
{
|
||||
|
||||
int32_t leftSample = (sample[LEFTCHANNEL] << BQ_SHIFT) / 2;
|
||||
int32_t rightSample = (sample[RIGHTCHANNEL] << BQ_SHIFT) / 2;
|
||||
|
||||
int64_t leftOutput = ((leftSample * i_a0) >> BQ_SHIFT) + i_lz1;
|
||||
i_lz1 = ((leftSample * i_a1) >> BQ_SHIFT) + i_lz2 - ((i_b1 * leftOutput) >> BQ_SHIFT);
|
||||
i_lz2 = ((leftSample * i_a2) >> BQ_SHIFT) - ((i_b2 * leftOutput) >> BQ_SHIFT);
|
||||
|
||||
int64_t rightOutput = ((rightSample * i_a0) >> BQ_SHIFT) + i_rz1;
|
||||
i_rz1 = ((rightSample * i_a1) >> BQ_SHIFT) + i_rz2 - ((i_b1 * rightOutput) >> BQ_SHIFT);
|
||||
i_rz2 = ((rightSample * i_a2) >> BQ_SHIFT) - ((i_b2 * rightOutput) >> BQ_SHIFT);
|
||||
|
||||
int16_t out[2];
|
||||
out[LEFTCHANNEL] = (int16_t)(leftOutput >> BQ_SHIFT);
|
||||
out[RIGHTCHANNEL] = (int16_t)(rightOutput >> BQ_SHIFT);
|
||||
|
||||
return sink->ConsumeSample(out);
|
||||
}
|
||||
|
||||
bool AudioOutputFilterBiquad::stop()
|
||||
{
|
||||
return sink->stop();
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
AudioOutputFilterBiquad
|
||||
Implements a Biquad filter
|
||||
|
||||
Copyright (C) 2012 Nigel Redmon
|
||||
Copyright (C) 2021 William Bérubé
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef _AudioOutputFilterBiquad_H
|
||||
#define _AudioOutputFilterBiquad_H
|
||||
|
||||
#include "AudioOutput.h"
|
||||
|
||||
#define BQ_SHIFT 16
|
||||
#define BQ_DECAL 65536
|
||||
|
||||
enum {
|
||||
bq_type_lowpass = 0,
|
||||
bq_type_highpass,
|
||||
bq_type_bandpass,
|
||||
bq_type_notch,
|
||||
bq_type_peak,
|
||||
bq_type_lowshelf,
|
||||
bq_type_highshelf
|
||||
};
|
||||
|
||||
class AudioOutputFilterBiquad : public AudioOutput
|
||||
{
|
||||
public:
|
||||
AudioOutputFilterBiquad(AudioOutput *sink);
|
||||
AudioOutputFilterBiquad(int type, float Fc, float Q, float peakGain, AudioOutput *sink);
|
||||
virtual ~AudioOutputFilterBiquad() override;
|
||||
virtual bool SetRate(int hz) override;
|
||||
virtual bool SetBitsPerSample(int bits) override;
|
||||
virtual bool SetChannels(int chan) override;
|
||||
virtual bool SetGain(float f) override;
|
||||
virtual bool begin() override;
|
||||
virtual bool ConsumeSample(int16_t sample[2]) override;
|
||||
virtual bool stop() override;
|
||||
|
||||
private:
|
||||
void SetType(int type);
|
||||
void SetFc(float Fc);
|
||||
void SetQ(float Q);
|
||||
void SetPeakGain(float peakGain);
|
||||
void SetBiquad(int type, float Fc, float Q, float peakGain);
|
||||
|
||||
protected:
|
||||
AudioOutput *sink;
|
||||
int buffSize;
|
||||
int16_t *leftSample;
|
||||
int16_t *rightSample;
|
||||
int writePtr;
|
||||
int readPtr;
|
||||
bool filled;
|
||||
int type;
|
||||
void CalcBiquad();
|
||||
int64_t i_a0, i_a1, i_a2, i_b1, i_b2;
|
||||
int64_t i_Fc, i_Q, i_peakGain;
|
||||
int64_t i_lz1, i_lz2, i_rz1, i_rz2;
|
||||
float a0, a1, a2, b1, b2;
|
||||
float Fc, Q, peakGain;
|
||||
float z1, z2;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Reference in new issue
Block a user