| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /* | |||
| 2 | * @file biquad.cpp | |||
| 3 | * | |||
| 4 | * @date Sep 10, 2016 | |||
| 5 | * @author Andrey Belomutskiy, (c) 2012-2020 | |||
| 6 | */ | |||
| 7 | ||||
| 8 | #include "pch.h" | |||
| 9 | ||||
| 10 | #include "biquad.h" | |||
| 11 | ||||
| 12 | 720 | Biquad::Biquad() { | ||
| 13 | // Default to passthru | |||
| 14 | 720 | a0 = 1; | ||
| 15 | 720 | a1 = a2 = b1 = b2 = 0; | ||
| 16 | ||||
| 17 | 720 | reset(); | ||
| 18 | 720 | } | ||
| 19 | ||||
| 20 | 720 | void Biquad::reset() { | ||
| 21 | 720 | z1 = z2 = 0; | ||
| 22 | 720 | } | ||
| 23 | ||||
| 24 | 601 | static float getK(float samplingFrequency, float cutoff) { | ||
| 25 | 601 | return tanf_taylor(CONST_PI * cutoff / samplingFrequency); | ||
| 26 | } | |||
| 27 | ||||
| 28 | 601 | static float getNorm(float K, float Q) { | ||
| 29 | 601 | return 1 / (1 + K / Q + K * K); | ||
| 30 | } | |||
| 31 | ||||
| 32 | ✗ | void Biquad::configureBandpass(float samplingFrequency, float centerFrequency, float Q) { | ||
| 33 | ✗ | if (samplingFrequency < 2.5f * centerFrequency) { | ||
| 34 | ✗ | criticalError("Invalid biquad parameters samplingFrequency=%f centerFrequency=%f", samplingFrequency, centerFrequency); | ||
| 35 | ✗ | return; | ||
| 36 | } | |||
| 37 | ||||
| 38 | ✗ | float K = getK(samplingFrequency, centerFrequency); | ||
| 39 | ✗ | float norm = getNorm(K, Q); | ||
| 40 | ||||
| 41 | ✗ | a0 = K / Q * norm; | ||
| 42 | ✗ | a1 = 0; | ||
| 43 | ✗ | a2 = -a0; | ||
| 44 | ✗ | b1 = 2 * (K * K - 1) * norm; | ||
| 45 | ✗ | b2 = (1 - K / Q + K * K) * norm; | ||
| 46 | } | |||
| 47 | ||||
| 48 | 13 | void Biquad::configureLowpass(float samplingFrequency, float cutoffFrequency, float Q) { | ||
| 49 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
|
13 | criticalAssertVoid(samplingFrequency >= 2.5f * cutoffFrequency, "Invalid biquad parameters"); | |
| 50 | ||||
| 51 | 13 | float K = getK(samplingFrequency, cutoffFrequency); | ||
| 52 | 13 | float norm = getNorm(K, Q); | ||
| 53 | ||||
| 54 | 13 | a0 = K * K * norm; | ||
| 55 | 13 | a1 = 2 * a0; | ||
| 56 | 13 | a2 = a0; | ||
| 57 | 13 | b1 = 2 * (K * K - 1) * norm; | ||
| 58 | 13 | b2 = (1 - K / Q + K * K) * norm; | ||
| 59 | } | |||
| 60 | ||||
| 61 | 588 | void Biquad::configureHighpass(float samplingFrequency, float cutoffFrequency, float Q) { | ||
| 62 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 588 times.
|
588 | criticalAssertVoid(samplingFrequency >= 2.5f * cutoffFrequency, "Invalid biquad parameters"); | |
| 63 | ||||
| 64 | 588 | float K = getK(samplingFrequency, cutoffFrequency); | ||
| 65 | 588 | float norm = getNorm(K, Q); | ||
| 66 | ||||
| 67 | 588 | a0 = 1 * norm; | ||
| 68 | 588 | a1 = -2 * a0; | ||
| 69 | 588 | a2 = a0; | ||
| 70 | 588 | b1 = 2 * (K * K - 1) * norm; | ||
| 71 | 588 | b2 = (1 - K / Q + K * K) * norm; | ||
| 72 | } | |||
| 73 | ||||
| 74 | 13007 | float Biquad::filter(float input) { | ||
| 75 | 13007 | float result = input * a0 + z1; | ||
| 76 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 13007 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 13007 times.
|
13007 | if (engineConfiguration->verboseQuad) { |
| 77 | ✗ | efiPrintf("input %f, a0 %f, z1 %f, result %f", input, a0, z1, result); | ||
| 78 | } | |||
| 79 | 13007 | z1 = input * a1 + z2 - b1 * result; | ||
| 80 | 13007 | z2 = input * a2 - b2 * result; | ||
| 81 | 13007 | return result; | ||
| 82 | } | |||
| 83 | ||||
| 84 | 3 | void Biquad::cookSteadyState(float steadyStateInput) { | ||
| 85 | 3 | float Y = steadyStateInput * (a0 + a1 + a2) / (1 + b1 + b2); | ||
| 86 | ||||
| 87 | 3 | float steady_z2 = steadyStateInput * a2 - Y * b2; | ||
| 88 | 3 | float steady_z1 = steady_z2 + steadyStateInput * a1 - Y * b1; | ||
| 89 | ||||
| 90 | 3 | this->z1 = steady_z1; | ||
| 91 | 3 | this->z2 = steady_z2; | ||
| 92 | 3 | } | ||
| 93 |