flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-04-21 21:05:31 +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 <QtMath>
#include <algorithm> #include <algorithm>
// return buffer index float BiQuadFilter::transform(float inSample)
#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)
{ {
clear(); // compute result
calculate(); 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; // precompute the coefficients
a0 = b0 / aa0;
// put input on to buffer a1 = b1 / aa0;
m_X[BUFFIX(n,0)] = input; a2 = b2 / aa0;
a3 = aa1 / aa0;
// process input a4 = aa2 / aa0;
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;
} }
void BiQuadFilter::clear() void BiQuadFilter::setLowPassFilter(float sampleRate, float cutoffFrequency, float q)
{ {
for (int n = 0; n < BQN; n++) // H(s) = 1 / (s^2 + s/Q + 1)
{ auto w0 = 2 * M_PI * cutoffFrequency / sampleRate;
m_X[n] = 0.0; auto cosw0 = qCos(w0);
m_Y[n] = 0.0; 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); // H(s) = (s^2 + s*(A/Q) + 1) / (s^2 + s/(A*Q) + 1)
double w0 = 2.0 * M_PI * m_fc / m_fs; auto w0 = 2 * M_PI * centreFrequency / sampleRate;
double alpha = qSin(w0) / (2.0 * m_Q); 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); auto b0 = 1 + alpha * a;
double sqrt_AA = qSqrt(AA); auto b1 = -2 * cosw0;
auto b2 = 1 - alpha * a;
// source : http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt auto aa0 = 1 + alpha / a;
auto aa1 = -2 * cosw0;
switch (m_type) { auto aa2 = 1 - alpha / a;
case BiQuadFilterType::LowPass: setCoefficients(aa0, aa1, aa2, b0, b1, b2);
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; void BiQuadFilter::setHighPassFilter(float sampleRate, float cutoffFrequency, float q)
m_A[0] = 1 + alpha; {
m_A[1] = -2.0 * cos_w0; // H(s) = s^2 / (s^2 + s/Q + 1)
m_A[2] = 1.0 - alpha; auto w0 = 2 * M_PI * cutoffFrequency / sampleRate;
break; auto cosw0 = qCos(w0);
case BiQuadFilterType::HighPass: auto alpha = qSin(w0) / (2 * q);
m_B[0] = (1.0 + cos_w0) / 2.0;
m_B[1] = -(1.0 + cos_w0); auto b0 = (1 + cosw0) / 2;
m_B[2] = (1.0 + cos_w0) / 2.0; auto b1 = -(1 + cosw0);
m_A[0] = 1.0 + alpha; auto b2 = (1 + cosw0) / 2;
m_A[1] = -2.0 * cos_w0; auto aa0 = 1 + alpha;
m_A[2] = 1.0 - alpha; auto aa1 = -2 * cosw0;
break; auto aa2 = 1 - alpha;
case BiQuadFilterType::BandPass: // (constant 0 dB peak gain) setCoefficients(aa0, aa1, aa2, b0, b1, b2);
m_B[0] = alpha; }
m_B[1] = 0.0;
m_B[2] = -alpha; BiQuadFilter BiQuadFilter::lowPassFilter(float sampleRate, float cutoffFrequency, float q)
m_A[0] = 1.0 + alpha; {
m_A[1] = -2.0 * cos_w0; BiQuadFilter filter;
m_A[2] = 1.0 - alpha; filter.setLowPassFilter(sampleRate, cutoffFrequency, q);
break; return filter;
case BiQuadFilterType::Notch: }
m_B[0] = 1.0;
m_B[1] = -2.0 * cos_w0; BiQuadFilter BiQuadFilter::highPassFilter(float sampleRate, float cutoffFrequency, float q)
m_B[2] = 1.0; {
m_A[0] = 1.0 + alpha; BiQuadFilter filter;
m_A[1] = -2.0 * cos_w0; filter.setHighPassFilter(sampleRate, cutoffFrequency, q);
m_A[2] = 1.0 - alpha; return filter;
break; }
case BiQuadFilterType::Peak:
m_B[0] = 1.0 + alpha*AA; BiQuadFilter BiQuadFilter::peakingEQ(float sampleRate, float centreFrequency, float q, float dbGain)
m_B[1] = -2.0 * cos_w0; {
m_B[2] = 1.0 - alpha*AA; BiQuadFilter filter;
m_A[0] = 1.0 + alpha/AA; filter.setPeakingEq(sampleRate, centreFrequency, q, dbGain);
m_A[1] = -2.0 * cos_w0; return filter;
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;
}
} }

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

@@ -3,48 +3,33 @@
#include "blacksound/blacksoundexport.h" #include "blacksound/blacksoundexport.h"
// filter type enumeration
enum class BiQuadFilterType {
None,
LowPass,
HighPass,
BandPass,
Notch,
Peak,
LowShelf,
HighShelf
};
#define BQN 3
class BiQuadFilter class BiQuadFilter
{ {
public: public:
BiQuadFilter() = default; BiQuadFilter() = default;
BiQuadFilter(BiQuadFilterType type, float transform(float inSample);
int fs = 44100, void setCoefficients(double aa0, double aa1, double aa2, double b0, double b1, double b2);
double fc = 1000, void setLowPassFilter(float sampleRate, float cutoffFrequency, float q);
double Q = 0.7071, void setPeakingEq(float sampleRate, float centreFrequency, float q, float dbGain);
double peakGain = 0); void setHighPassFilter(float sampleRate, float cutoffFrequency, float q);
float process(float input); static BiQuadFilter lowPassFilter(float sampleRate, float cutoffFrequency, float q);
static BiQuadFilter highPassFilter(float sampleRate, float cutoffFrequency, float q);
void clear(); static BiQuadFilter peakingEQ(float sampleRate, float centreFrequency, float q, float dbGain);
private: private:
void calculate(); double a0;
double a1;
double a2;
double a3;
double a4;
double m_A[BQN]; // state
double m_B[BQN]; float x1 = 0.0;
float m_X[BQN]; float x2 = 0.0;
float m_Y[BQN]; float y1 = 0.0;
unsigned int m_index = 0; float y2 = 0.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;
}; };
#endif // BIQUADFILTER_H #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++) 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; doubleSamples[n] *= m_outputGain;
} }
@@ -40,11 +40,11 @@ namespace BlackSound
switch (preset) switch (preset)
{ {
case VHFEmulation: case VHFEmulation:
m_filters.push_back(BiQuadFilter(BiQuadFilterType::HighPass, 44100, 310, 0.25)); m_filters.push_back(BiQuadFilter::highPassFilter(44100, 310, 0.25));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 450, 0.75, 17.0)); m_filters.push_back(BiQuadFilter::peakingEQ(44100, 450, 0.75, 17.0));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 1450, 1.0, 25.0)); m_filters.push_back(BiQuadFilter::peakingEQ(44100, 1450, 1.0, 25.0));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 2000, 1.0, 25.0)); m_filters.push_back(BiQuadFilter::peakingEQ(44100, 2000, 1.0, 25.0));
m_filters.push_back(BiQuadFilter(BiQuadFilterType::LowPass, 44100, 2500, 0.25)); m_filters.push_back(BiQuadFilter::lowPassFilter(44100, 2500, 0.25));
break; break;
} }
} }