|
|
@ -25,7 +25,6 @@ |
|
|
|
#include "mathDSP_F32.h" |
|
|
|
#include "mathDSP_F32.h" |
|
|
|
#include "basic_components.h" |
|
|
|
#include "basic_components.h" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class AudioFilterEqualizer3band_F32 : public AudioStream_F32 |
|
|
|
class AudioFilterEqualizer3band_F32 : public AudioStream_F32 |
|
|
|
{ |
|
|
|
{ |
|
|
|
public: |
|
|
|
public: |
|
|
@ -33,14 +32,14 @@ public: |
|
|
|
{ |
|
|
|
{ |
|
|
|
setBands(500.0f, 3000.0f); |
|
|
|
setBands(500.0f, 3000.0f); |
|
|
|
} |
|
|
|
} |
|
|
|
void setBands(float32_t bassF, float32_t trebleF) |
|
|
|
void setBands(float32_t bassF, float32_t trebleF) |
|
|
|
{ |
|
|
|
{ |
|
|
|
trebleF = 2.0f * sinf(M_PI * (trebleF / AUDIO_SAMPLE_RATE_EXACT)); |
|
|
|
trebleF = 2.0f * sinf(M_PI * (trebleF / AUDIO_SAMPLE_RATE_EXACT)); |
|
|
|
bassF = 2.0f * sinf(M_PI * (bassF / AUDIO_SAMPLE_RATE_EXACT)); |
|
|
|
bassF = 2.0f * sinf(M_PI * (bassF / AUDIO_SAMPLE_RATE_EXACT)); |
|
|
|
|
|
|
|
|
|
|
|
__disable_irq(); |
|
|
|
__disable_irq(); |
|
|
|
lowpass_f = bassF; |
|
|
|
lowpass_f = bassF; |
|
|
|
hipass_f = trebleF;
|
|
|
|
hipass_f = trebleF; |
|
|
|
__enable_irq(); |
|
|
|
__enable_irq(); |
|
|
|
} |
|
|
|
} |
|
|
|
void treble(float32_t t) |
|
|
|
void treble(float32_t t) |
|
|
@ -66,7 +65,7 @@ public: |
|
|
|
{ |
|
|
|
{ |
|
|
|
__disable_irq(); |
|
|
|
__disable_irq(); |
|
|
|
treble_g = t; |
|
|
|
treble_g = t; |
|
|
|
mid_g = m; |
|
|
|
mid_g = m; |
|
|
|
bass_g = b; |
|
|
|
bass_g = b; |
|
|
|
__enable_irq(); |
|
|
|
__enable_irq(); |
|
|
|
} |
|
|
|
} |
|
|
@ -85,7 +84,7 @@ public: |
|
|
|
} |
|
|
|
} |
|
|
|
for (i = 0; i < block->length; i++) |
|
|
|
for (i = 0; i < block->length; i++) |
|
|
|
{ |
|
|
|
{ |
|
|
|
float32_t lpOut, midOut, hpOut; // Low / Mid / High - Sample Values
|
|
|
|
float32_t lpOut, midOut, hpOut; // Low / Mid / High - Sample Values
|
|
|
|
float32_t sample = block->data[i]; // give some headroom
|
|
|
|
float32_t sample = block->data[i]; // give some headroom
|
|
|
|
// Filter #1 (lowpass)
|
|
|
|
// Filter #1 (lowpass)
|
|
|
|
f1p0 += (lowpass_f * (sample - f1p0)) + vsa; |
|
|
|
f1p0 += (lowpass_f * (sample - f1p0)) + vsa; |
|
|
@ -96,9 +95,9 @@ public: |
|
|
|
|
|
|
|
|
|
|
|
// // Filter #2 (highpass)
|
|
|
|
// // Filter #2 (highpass)
|
|
|
|
f2p0 += (hipass_f * (sample - f2p0)) + vsa; |
|
|
|
f2p0 += (hipass_f * (sample - f2p0)) + vsa; |
|
|
|
f2p1 += (hipass_f * (f2p0 - f2p1)); |
|
|
|
f2p1 += (hipass_f * (f2p0 - f2p1)); |
|
|
|
f2p2 += (hipass_f * (f2p1 - f2p2)); |
|
|
|
f2p2 += (hipass_f * (f2p1 - f2p2)); |
|
|
|
f2p3 += (hipass_f * (f2p2 - f2p3)); |
|
|
|
f2p3 += (hipass_f * (f2p2 - f2p3)); |
|
|
|
hpOut = sdm3 - f2p3; |
|
|
|
hpOut = sdm3 - f2p3; |
|
|
|
|
|
|
|
|
|
|
|
midOut = sdm3 - (lpOut + hpOut); |
|
|
|
midOut = sdm3 - (lpOut + hpOut); |
|
|
@ -123,6 +122,7 @@ public: |
|
|
|
bp ^= 1; |
|
|
|
bp ^= 1; |
|
|
|
return bp; |
|
|
|
return bp; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
private: |
|
|
|
private: |
|
|
|
audio_block_f32_t *inputQueueArray[1]; |
|
|
|
audio_block_f32_t *inputQueueArray[1]; |
|
|
|
bool bp = false; |
|
|
|
bool bp = false; |
|
|
@ -136,9 +136,9 @@ private: |
|
|
|
float32_t f2p2 = 0.0f; |
|
|
|
float32_t f2p2 = 0.0f; |
|
|
|
float32_t f2p3 = 0.0f; |
|
|
|
float32_t f2p3 = 0.0f; |
|
|
|
|
|
|
|
|
|
|
|
float32_t sdm1 = 0.0f;
|
|
|
|
float32_t sdm1 = 0.0f; |
|
|
|
float32_t sdm2 = 0.0f; // 2
|
|
|
|
float32_t sdm2 = 0.0f; // 2
|
|
|
|
float32_t sdm3 = 0.0f; // 3
|
|
|
|
float32_t sdm3 = 0.0f; // 3
|
|
|
|
|
|
|
|
|
|
|
|
static constexpr float32_t vsa = (1.0 / 4294967295.0); // Very small amount (Denormal Fix)
|
|
|
|
static constexpr float32_t vsa = (1.0 / 4294967295.0); // Very small amount (Denormal Fix)
|
|
|
|
float32_t lpreg; |
|
|
|
float32_t lpreg; |
|
|
@ -148,6 +148,185 @@ private: |
|
|
|
float32_t hipass_f; |
|
|
|
float32_t hipass_f; |
|
|
|
float32_t treble_g = 1.0f; |
|
|
|
float32_t treble_g = 1.0f; |
|
|
|
float32_t mid_g = 1.0f; |
|
|
|
float32_t mid_g = 1.0f; |
|
|
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class AudioFilterEqualizer3bandStereo_F32 : public AudioStream_F32 |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
public: |
|
|
|
|
|
|
|
AudioFilterEqualizer3bandStereo_F32(void) : AudioStream_F32(2, inputQueueArray) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
setBands(500.0f, 3000.0f); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void setBands(float32_t bassF, float32_t trebleF) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
trebleF = 2.0f * sinf(M_PI * (trebleF / AUDIO_SAMPLE_RATE_EXACT)); |
|
|
|
|
|
|
|
bassF = 2.0f * sinf(M_PI * (bassF / AUDIO_SAMPLE_RATE_EXACT)); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
__disable_irq(); |
|
|
|
|
|
|
|
lowpass_f = bassF; |
|
|
|
|
|
|
|
hipass_f = trebleF; |
|
|
|
|
|
|
|
__enable_irq(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void treble(float32_t t) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
__disable_irq(); |
|
|
|
|
|
|
|
treble_g = t; |
|
|
|
|
|
|
|
__enable_irq(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void mid(float32_t m) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
__disable_irq(); |
|
|
|
|
|
|
|
mid_g = m; |
|
|
|
|
|
|
|
__enable_irq(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void bass(float32_t b) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
__disable_irq(); |
|
|
|
|
|
|
|
bass_g = b; |
|
|
|
|
|
|
|
__enable_irq(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void set(float32_t t, float32_t m, float32_t b) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
__disable_irq(); |
|
|
|
|
|
|
|
treble_g = t; |
|
|
|
|
|
|
|
mid_g = m; |
|
|
|
|
|
|
|
bass_g = b; |
|
|
|
|
|
|
|
__enable_irq(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void update() |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
audio_block_f32_t *blockL, *blockR; |
|
|
|
|
|
|
|
int i; |
|
|
|
|
|
|
|
if (bp) // bypass mode
|
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
blockL = AudioStream_F32::receiveReadOnly_f32(0); |
|
|
|
|
|
|
|
blockR = AudioStream_F32::receiveReadOnly_f32(1); |
|
|
|
|
|
|
|
if (!blockL || !blockR) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
if (blockL) |
|
|
|
|
|
|
|
AudioStream_F32::release(blockL); |
|
|
|
|
|
|
|
if (blockR) |
|
|
|
|
|
|
|
AudioStream_F32::release(blockR); |
|
|
|
|
|
|
|
return; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
AudioStream_F32::transmit(blockL, 0); |
|
|
|
|
|
|
|
AudioStream_F32::transmit(blockR, 1); |
|
|
|
|
|
|
|
AudioStream_F32::release(blockL); |
|
|
|
|
|
|
|
AudioStream_F32::release(blockR); |
|
|
|
|
|
|
|
return; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
blockL = AudioStream_F32::receiveWritable_f32(0); |
|
|
|
|
|
|
|
blockR = AudioStream_F32::receiveWritable_f32(1); |
|
|
|
|
|
|
|
if (!blockL || !blockR) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
if (blockL) |
|
|
|
|
|
|
|
AudioStream_F32::release(blockL); |
|
|
|
|
|
|
|
if (blockR) |
|
|
|
|
|
|
|
AudioStream_F32::release(blockR); |
|
|
|
|
|
|
|
return; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (i = 0; i < blockL->length; i++) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
float32_t lpOut, midOut, hpOut; // Low / Mid / High - Sample Values
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// channel L
|
|
|
|
|
|
|
|
float32_t sample = blockL->data[i]; // give some headroom
|
|
|
|
|
|
|
|
// Filter #1 (lowpass)
|
|
|
|
|
|
|
|
f1p0[0] += (lowpass_f * (sample - f1p0[0])) + vsa; |
|
|
|
|
|
|
|
f1p1[0] += (lowpass_f * (f1p0[0] - f1p1[0])); |
|
|
|
|
|
|
|
f1p2[0] += (lowpass_f * (f1p1[0] - f1p2[0])); |
|
|
|
|
|
|
|
f1p3[0] += (lowpass_f * (f1p2[0] - f1p3[0])); |
|
|
|
|
|
|
|
lpOut = f1p3[0]; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// // Filter #2 (highpass)
|
|
|
|
|
|
|
|
f2p0[0] += (hipass_f * (sample - f2p0[0])) + vsa; |
|
|
|
|
|
|
|
f2p1[0] += (hipass_f * (f2p0[0] - f2p1[0])); |
|
|
|
|
|
|
|
f2p2[0] += (hipass_f * (f2p1[0] - f2p2[0])); |
|
|
|
|
|
|
|
f2p3[0] += (hipass_f * (f2p2[0] - f2p3[0])); |
|
|
|
|
|
|
|
hpOut = sdm3[0] - f2p3[0]; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
midOut = sdm3[0] - (lpOut + hpOut); |
|
|
|
|
|
|
|
// // Scale, Combine and store
|
|
|
|
|
|
|
|
lpOut *= bass_g; |
|
|
|
|
|
|
|
midOut *= mid_g; |
|
|
|
|
|
|
|
hpOut *= treble_g; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// // Shuffle history buffer
|
|
|
|
|
|
|
|
sdm3[0] = sdm2[0]; |
|
|
|
|
|
|
|
sdm2[0] = sdm1[0]; |
|
|
|
|
|
|
|
sdm1[0] = sample; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
blockL->data[i] = (lpOut + midOut + hpOut); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// channel R
|
|
|
|
|
|
|
|
sample = blockR->data[i]; // give some headroom
|
|
|
|
|
|
|
|
// Filter #1 (lowpass)
|
|
|
|
|
|
|
|
f1p0[1] += (lowpass_f * (sample - f1p0[1])) + vsa; |
|
|
|
|
|
|
|
f1p1[1] += (lowpass_f * (f1p0[1] - f1p1[1])); |
|
|
|
|
|
|
|
f1p2[1] += (lowpass_f * (f1p1[1] - f1p2[1])); |
|
|
|
|
|
|
|
f1p3[1] += (lowpass_f * (f1p2[1] - f1p3[1])); |
|
|
|
|
|
|
|
lpOut = f1p3[1]; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// // Filter #2 (highpass)
|
|
|
|
|
|
|
|
f2p0[1] += (hipass_f * (sample - f2p0[1])) + vsa; |
|
|
|
|
|
|
|
f2p1[1] += (hipass_f * (f2p0[1] - f2p1[1])); |
|
|
|
|
|
|
|
f2p2[1] += (hipass_f * (f2p1[1] - f2p2[1])); |
|
|
|
|
|
|
|
f2p3[1] += (hipass_f * (f2p2[1] - f2p3[1])); |
|
|
|
|
|
|
|
hpOut = sdm3[1] - f2p3[1]; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
midOut = sdm3[1] - (lpOut + hpOut); |
|
|
|
|
|
|
|
// // Scale, Combine and store
|
|
|
|
|
|
|
|
lpOut *= bass_g; |
|
|
|
|
|
|
|
midOut *= mid_g; |
|
|
|
|
|
|
|
hpOut *= treble_g; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// // Shuffle history buffer
|
|
|
|
|
|
|
|
sdm3[1] = sdm2[1]; |
|
|
|
|
|
|
|
sdm2[1] = sdm1[1]; |
|
|
|
|
|
|
|
sdm1[1] = sample; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
blockR->data[i] = (lpOut + midOut + hpOut);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
AudioStream_F32::transmit(blockL, 0); |
|
|
|
|
|
|
|
AudioStream_F32::transmit(blockR, 1); |
|
|
|
|
|
|
|
AudioStream_F32::release(blockL); |
|
|
|
|
|
|
|
AudioStream_F32::release(blockR); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void bypass_set(bool s) { bp = s; } |
|
|
|
|
|
|
|
bool bypass_tgl() |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
bp ^= 1; |
|
|
|
|
|
|
|
return bp; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
private: |
|
|
|
|
|
|
|
audio_block_f32_t *inputQueueArray[2]; |
|
|
|
|
|
|
|
bool bp = false; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float32_t f1p0[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f1p1[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f1p2[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f1p3[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f2p0[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f2p1[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f2p2[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t f2p3[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float32_t sdm1[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t sdm2[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
float32_t sdm3[2] = {0.0f, 0.0f}; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static constexpr float32_t vsa = (1.0 / 4294967295.0); // Very small amount (Denormal Fix)
|
|
|
|
|
|
|
|
float32_t lpreg[2]; |
|
|
|
|
|
|
|
float32_t hpreg[2]; |
|
|
|
|
|
|
|
float32_t lowpass_f; |
|
|
|
|
|
|
|
float32_t bass_g = 1.0f; |
|
|
|
|
|
|
|
float32_t hipass_f; |
|
|
|
|
|
|
|
float32_t treble_g = 1.0f; |
|
|
|
|
|
|
|
float32_t mid_g = 1.0f; |
|
|
|
}; |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
#endif |
|
|
|
#endif |