flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-04-02 23:25:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

[AFV] Port NAudio BiQuadFilter

Browse Source
This commit is contained in:
Roland Rossgotterer
2019-10-01 09:46:18 +02:00
committed by Mat Sutcliffe
parent 574370579d
commit ec68e756cd
3 changed files with 108 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

203
src/blacksound/dsp/biquadfilter.cpp
View File

@@ -3,134 +3,99 @@
#include <QtMath>
#include <algorithm>
// return buffer index
#define BUFFIX(n,k) ((n + k + BQN) % BQN)
BiQuadFilter::BiQuadFilter(BiQuadFilterType type, int fs, double fc, double Q, double peakGain) :
m_fs(fs),
m_type(type),
m_fc(fc),
m_Q(Q),
m_peakGain(peakGain)
float BiQuadFilter::transform(float inSample)
{
clear();
calculate();
// compute result
double result = a0 * inSample + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2;
// shift x1 to x2, sample to x1
x2 = x1;
x1 = inSample;
// shift y1 to y2, result to y1
y2 = y1;
y1 = (float)result;
return y1;
}
float BiQuadFilter::process(float input)
void BiQuadFilter::setCoefficients(double aa0, double aa1, double aa2, double b0, double b1, double b2)
{
unsigned int n = m_index;
// put input on to buffer
m_X[BUFFIX(n,0)] = input;
// process input
m_Y[BUFFIX(n,0)] =
m_B[0] * m_X[BUFFIX(n, 0)] +
m_B[1] * m_X[BUFFIX(n, -1)] +
m_B[2] * m_X[BUFFIX(n, -2)] -
m_A[1] * m_Y[BUFFIX(n, -1)] -
m_A[2] * m_Y[BUFFIX(n, -2)];
// write output
float output = m_Y[BUFFIX(n, 0)];
// step through buffer
m_index = BUFFIX(n, 1);
return output;
// precompute the coefficients
a0 = b0 / aa0;
a1 = b1 / aa0;
a2 = b2 / aa0;
a3 = aa1 / aa0;
a4 = aa2 / aa0;
}
void BiQuadFilter::clear()
void BiQuadFilter::setLowPassFilter(float sampleRate, float cutoffFrequency, float q)
{
for (int n = 0; n < BQN; n++)
{
m_X[n] = 0.0;
m_Y[n] = 0.0;
}
// H(s) = 1 / (s^2 + s/Q + 1)
auto w0 = 2 * M_PI * cutoffFrequency / sampleRate;
auto cosw0 = qCos(w0);
auto alpha = qSin(w0) / (2 * q);
auto b0 = (1 - cosw0) / 2;
auto b1 = 1 - cosw0;
auto b2 = (1 - cosw0) / 2;
auto aa0 = 1 + alpha;
auto aa1 = -2 * cosw0;
auto aa2 = 1 - alpha;
setCoefficients(aa0, aa1, aa2, b0, b1, b2);
}
void BiQuadFilter::calculate()
void BiQuadFilter::setPeakingEq(float sampleRate, float centreFrequency, float q, float dbGain)
{
double AA = qPow(10.0, m_peakGain / 40.0);
double w0 = 2.0 * M_PI * m_fc / m_fs;
double alpha = qSin(w0) / (2.0 * m_Q);
// H(s) = (s^2 + s*(A/Q) + 1) / (s^2 + s/(A*Q) + 1)
auto w0 = 2 * M_PI * centreFrequency / sampleRate;
auto cosw0 = qCos(w0);
auto sinw0 = qSin(w0);
auto alpha = sinw0 / (2 * q);
auto a = qPow(10, dbGain / 40); // TODO: should we square root this value?
double cos_w0 = qCos(w0);
double sqrt_AA = qSqrt(AA);
// source : http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
switch (m_type) {
case BiQuadFilterType::LowPass:
m_B[0] = (1.0 - cos_w0) / 2.0;
m_B[1] = 1.0 - cos_w0;
m_B[2] = (1.0 - cos_w0) / 2.0;
m_A[0] = 1 + alpha;
m_A[1] = -2.0 * cos_w0;
m_A[2] = 1.0 - alpha;
break;
case BiQuadFilterType::HighPass:
m_B[0] = (1.0 + cos_w0) / 2.0;
m_B[1] = -(1.0 + cos_w0);
m_B[2] = (1.0 + cos_w0) / 2.0;
m_A[0] = 1.0 + alpha;
m_A[1] = -2.0 * cos_w0;
m_A[2] = 1.0 - alpha;
break;
case BiQuadFilterType::BandPass: // (constant 0 dB peak gain)
m_B[0] = alpha;
m_B[1] = 0.0;
m_B[2] = -alpha;
m_A[0] = 1.0 + alpha;
m_A[1] = -2.0 * cos_w0;
m_A[2] = 1.0 - alpha;
break;
case BiQuadFilterType::Notch:
m_B[0] = 1.0;
m_B[1] = -2.0 * cos_w0;
m_B[2] = 1.0;
m_A[0] = 1.0 + alpha;
m_A[1] = -2.0 * cos_w0;
m_A[2] = 1.0 - alpha;
break;
case BiQuadFilterType::Peak:
m_B[0] = 1.0 + alpha*AA;
m_B[1] = -2.0 * cos_w0;
m_B[2] = 1.0 - alpha*AA;
m_A[0] = 1.0 + alpha/AA;
m_A[1] = -2.0 * cos_w0;
m_A[2] = 1.0 - alpha/AA;
break;
case BiQuadFilterType::LowShelf:
m_B[0] = AA*( (AA+1.0) - (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha );
m_B[1] = 2.0*AA*( (AA-1) - (AA+1.0) * cos_w0 );
m_B[2] = AA*( (AA+1.0) - (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha );
m_A[0] = (AA+1.0) + (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha;
m_A[1] = -2.0*( (AA-1.0) + (AA+1.0) * cos_w0 );
m_A[2] = (AA+1.0) + (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha;
break;
case BiQuadFilterType::HighShelf:
m_B[0] = AA*( (AA+1.0) + (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha );
m_B[1] = -2.0*AA*( (AA-1.0) + (AA+1.0) * cos_w0 );
m_B[2] = AA*( (AA+1.0) + (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha );
m_A[0] = (AA+1.0) - (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha;
m_A[1] = 2.0*( (AA-1.0) - (AA+1.0) * cos_w0 );
m_A[2] = (AA+1.0) - (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha;
break;
case BiQuadFilterType::None:
m_B[0] = 1.0;
m_B[1] = 0.0;
m_B[2] = 0.0;
m_A[0] = 1.0;
m_A[1] = 0.0;
m_A[2] = 0.0;
}
// normalize
double norm = m_A[0];
for (int i = 0; i < BQN; i++) {
m_A[i] /= norm;
m_B[i] /= norm;
}
auto b0 = 1 + alpha * a;
auto b1 = -2 * cosw0;
auto b2 = 1 - alpha * a;
auto aa0 = 1 + alpha / a;
auto aa1 = -2 * cosw0;
auto aa2 = 1 - alpha / a;
setCoefficients(aa0, aa1, aa2, b0, b1, b2);
}
void BiQuadFilter::setHighPassFilter(float sampleRate, float cutoffFrequency, float q)
{
// H(s) = s^2 / (s^2 + s/Q + 1)
auto w0 = 2 * M_PI * cutoffFrequency / sampleRate;
auto cosw0 = qCos(w0);
auto alpha = qSin(w0) / (2 * q);
auto b0 = (1 + cosw0) / 2;
auto b1 = -(1 + cosw0);
auto b2 = (1 + cosw0) / 2;
auto aa0 = 1 + alpha;
auto aa1 = -2 * cosw0;
auto aa2 = 1 - alpha;
setCoefficients(aa0, aa1, aa2, b0, b1, b2);
}
BiQuadFilter BiQuadFilter::lowPassFilter(float sampleRate, float cutoffFrequency, float q)
{
BiQuadFilter filter;
filter.setLowPassFilter(sampleRate, cutoffFrequency, q);
return filter;
}
BiQuadFilter BiQuadFilter::highPassFilter(float sampleRate, float cutoffFrequency, float q)
{
BiQuadFilter filter;
filter.setHighPassFilter(sampleRate, cutoffFrequency, q);
return filter;
}
BiQuadFilter BiQuadFilter::peakingEQ(float sampleRate, float centreFrequency, float q, float dbGain)
{
BiQuadFilter filter;
filter.setPeakingEq(sampleRate, centreFrequency, q, dbGain);
return filter;
}

51
src/blacksound/dsp/biquadfilter.h
View File

@@ -3,48 +3,33 @@
#include "blacksound/blacksoundexport.h"
// filter type enumeration
enum class BiQuadFilterType {
None,
LowPass,
HighPass,
BandPass,
Notch,
Peak,
LowShelf,
HighShelf
};
#define BQN 3
class BiQuadFilter
{
public:
BiQuadFilter() = default;
BiQuadFilter(BiQuadFilterType type,
int fs = 44100,
double fc = 1000,
double Q = 0.7071,
double peakGain = 0);
float transform(float inSample);
void setCoefficients(double aa0, double aa1, double aa2, double b0, double b1, double b2);
void setLowPassFilter(float sampleRate, float cutoffFrequency, float q);
void setPeakingEq(float sampleRate, float centreFrequency, float q, float dbGain);
void setHighPassFilter(float sampleRate, float cutoffFrequency, float q);
float process(float input);
void clear();
static BiQuadFilter lowPassFilter(float sampleRate, float cutoffFrequency, float q);
static BiQuadFilter highPassFilter(float sampleRate, float cutoffFrequency, float q);
static BiQuadFilter peakingEQ(float sampleRate, float centreFrequency, float q, float dbGain);
private:
void calculate();
double a0;
double a1;
double a2;
double a3;
double a4;
double m_A[BQN];
double m_B[BQN];
float m_X[BQN];
float m_Y[BQN];
unsigned int m_index = 0;
double m_fs = 44100.0;
BiQuadFilterType m_type = BiQuadFilterType::None;
double m_fc = 1000.0;
double m_Q = 0.7071;
double m_peakGain = 0.0;
// state
float x1 = 0.0;
float x2 = 0.0;
float y1 = 0.0;
float y2 = 0.0;
};
#endif // BIQUADFILTER_H

12
src/blacksound/sampleprovider/equalizersampleprovider.cpp
View File

@@ -26,7 +26,7 @@ namespace BlackSound
for (int band = 0; band < m_filters.size(); band++)
{
doubleSamples[n] = m_filters[band].process(doubleSamples[n]);
doubleSamples[n] = m_filters[band].transform(doubleSamples[n]);
}
doubleSamples[n] *= m_outputGain;
}
@@ -40,11 +40,11 @@ namespace BlackSound
switch (preset)
{
case VHFEmulation:
m_filters.push_back(BiQuadFilter(BiQuadFilterType::HighPass, 44100, 310, 0.25));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 450, 0.75, 17.0));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 1450, 1.0, 25.0));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 2000, 1.0, 25.0));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::LowPass, 44100, 2500, 0.25));
m_filters.push_back(BiQuadFilter::highPassFilter(44100, 310, 0.25));
m_filters.push_back(BiQuadFilter::peakingEQ(44100, 450, 0.75, 17.0));
m_filters.push_back(BiQuadFilter::peakingEQ(44100, 1450, 1.0, 25.0));
m_filters.push_back(BiQuadFilter::peakingEQ(44100, 2000, 1.0, 25.0));
m_filters.push_back(BiQuadFilter::lowPassFilter(44100, 2500, 0.25));
break;
}
}