| #include "filter_ladder.h" | #include "filter_ladder.h" | ||||
| #include <math.h> | #include <math.h> | ||||
| #include <stdint.h> | #include <stdint.h> | ||||
| #define MOOG_PI 3.14159265358979323846264338327950288 | |||||
| #define MOOG_PI ((float)3.14159265358979323846264338327950288) | |||||
| float AudioFilterLadder::LPF(float s, int i) | float AudioFilterLadder::LPF(float s, int i) | ||||
| { | { | ||||
| float ft = s * (1/1.3) + (0.3/1.3) * z0[i] - z1[i]; | |||||
| float ft = s * (1.0f/1.3f) + (0.3f/1.3f) * z0[i] - z1[i]; | |||||
| ft = ft * alpha + z1[i]; | ft = ft * alpha + z1[i]; | ||||
| z1[i] = ft; | z1[i] = ft; | ||||
| z0[i] = s; | z0[i] = s; | ||||
| void AudioFilterLadder::compute_coeffs(float c) | void AudioFilterLadder::compute_coeffs(float c) | ||||
| { | { | ||||
| if (c > 0.49 * AUDIO_SAMPLE_RATE_EXACT) | |||||
| c = 0.49 * AUDIO_SAMPLE_RATE_EXACT; | |||||
| if (c > 0.49f * AUDIO_SAMPLE_RATE_EXACT) | |||||
| c = 0.49f * AUDIO_SAMPLE_RATE_EXACT; | |||||
| else if (c < 1) | else if (c < 1) | ||||
| c = 1; | c = 1; | ||||
| float wc = c * (2.0 * MOOG_PI / AUDIO_SAMPLE_RATE_EXACT); | |||||
| float wc = c * (float)(2.0f * MOOG_PI / AUDIO_SAMPLE_RATE_EXACT); | |||||
| float wc2 = wc * wc; | float wc2 = wc * wc; | ||||
| alpha = 0.9892f * wc - 0.4324f * wc2 + 0.1381f * wc * wc2 - 0.0202f * wc2 * wc2; | alpha = 0.9892f * wc - 0.4324f * wc2 + 0.1381f * wc * wc2 - 0.0202f * wc2 * wc2; | ||||
| } | } | ||||
| static inline float fast_tanh(float x) | static inline float fast_tanh(float x) | ||||
| { | { | ||||
| float x2 = x * x; | float x2 = x * x; | ||||
| return x * (27.0 + x2) / (27.0 + 9.0 * x2); | |||||
| return x * (27.0f + x2) / (27.0f + 9.0f * x2); | |||||
| } | } | ||||
| void AudioFilterLadder::update(void) | void AudioFilterLadder::update(void) | ||||
| } | } | ||||
| if (!blockb) FCmodActive = false; | if (!blockb) FCmodActive = false; | ||||
| for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) { | for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) { | ||||
| float input = blocka->data[i] * (1.0/32768); | |||||
| float input = blocka->data[i] * (1.0f/32768.0f); | |||||
| ftot = Fbase; | ftot = Fbase; | ||||
| if (FCmodActive) { | if (FCmodActive) { | ||||
| FCmod = blockb->data[i] * (1.0/32768); | |||||
| FCmod = blockb->data[i] * (1.0f/32768.0f); | |||||
| ftot += Fbase * FCmod; | ftot += Fbase * FCmod; | ||||
| if (FCmod != 0) compute_coeffs(ftot); | if (FCmod != 0) compute_coeffs(ftot); | ||||
| } | } | ||||
| float u = input - (z1[3] - 0.5 * input) * K; | |||||
| float u = input - (z1[3] - 0.5f * input) * K; | |||||
| u = fast_tanh(u); | u = fast_tanh(u); | ||||
| float stage1 = LPF(u,0); | |||||
| float stage2 = LPF(stage1,1); | |||||
| float stage3 = LPF(stage2,2); | |||||
| float stage4 = LPF(stage3,3); | |||||
| blocka->data[i] = stage4 * 32767.0; | |||||
| float stage1 = LPF(u, 0); | |||||
| float stage2 = LPF(stage1, 1); | |||||
| float stage3 = LPF(stage2, 2); | |||||
| float stage4 = LPF(stage3, 3); | |||||
| blocka->data[i] = stage4 * 32767.0f; | |||||
| } | } | ||||
| transmit(blocka); | transmit(blocka); | ||||
| release(blocka); | release(blocka); |