diff options
Diffstat (limited to 'sos-iir-filter.h')
| -rw-r--r-- | sos-iir-filter.h | 128 |
1 files changed, 128 insertions, 0 deletions
diff --git a/sos-iir-filter.h b/sos-iir-filter.h new file mode 100644 index 0000000..cc90b35 --- /dev/null +++ b/sos-iir-filter.h @@ -0,0 +1,128 @@ +/* + * ESP32 Second-Order Sections IIR Filter implementation + * + * (c)2019 Ivan Kostoski + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + */ + +#ifndef SOS_IIR_FILTER_H +#define SOS_IIR_FILTER_H + +#include <algorithm> +#include <array> +#include <cstdint> +#include <ranges> +#include <utility> + +#include <cmath> +namespace fpm = std; +using sos_t = float; +//#include <fpm/math.hpp> +//using sos_t = fpm::fixed_16_16; + +struct SOS_Coefficients { + sos_t b1; + sos_t b2; + sos_t a1; + sos_t a2; +}; + +struct SOS_Delay_State { + sos_t w0; + sos_t w1; +}; + +/** + * Envelops above asm functions into C++ class + */ +template<std::size_t N> +struct SOS_IIR_Filter { + const sos_t gain; + std::array<SOS_Coefficients, N> sos; + std::array<SOS_Delay_State, N> w; + + // Template constructor for const filter declaration + constexpr SOS_IIR_Filter(const sos_t gain, const SOS_Coefficients (&_sos)[N]): + gain(gain) + { + std::copy(_sos, _sos + N, sos.begin()); + } + + void filter(auto samples) { + // Apply all but last Second-Order-Section + for ([[maybe_unused]] auto _ : std::views::zip_transform( + [](auto samps, const auto& coeffs, auto& ww) { + // Assumes a0 and b0 coefficients are one (1.0) + for (auto& s : samps) { + auto f6 = s + coeffs.a1 * ww.w0 + coeffs.a2 * ww.w1; + s = f6 + coeffs.b1 * ww.w0 + coeffs.b2 * ww.w1; + ww.w1 = std::exchange(ww.w0, f6); + } + return 0; + }, + std::views::repeat(samples, N), sos, w)) {} + } + + sos_t filter_sum_sqr(auto samples) { + filter(samples); + return std::ranges::fold_left(samples, sos_t(0), + [this](auto a, auto b) { return a + b * gain * b * gain; }); + } +}; + +#endif // SOS_IIR_FILTER_H + +// Knowles SPH0645LM4H-B, rev. B +// https://cdn-shop.adafruit.com/product-files/3421/i2S+Datasheet.PDF +// B ~= [1.001234, -1.991352, 0.990149] +// A ~= [1.0, -1.993853, 0.993863] +// With additional DC blocking component +SOS_IIR_Filter SPH0645LM4H_B_RB = { + /* gain: */ sos_t(1.00123377961525), + /* sos: */ { // Second-Order Sections {b1, b2, -a1, -a2} + { sos_t(-1.0), sos_t(0.0), + sos_t(+0.9992), sos_t(0) }, // DC blocker, a1 = -0.9992 + { sos_t(-1.988897663539382), sos_t(+0.988928479008099), + sos_t(+1.993853376183491), sos_t(-0.993862821429572) } } +}; + +// +// A-weighting IIR Filter, Fs = 48KHz +// (By Dr. Matt L., Source: https://dsp.stackexchange.com/a/36122) +// B = [0.169994948147430, 0.280415310498794, -1.120574766348363, 0.131562559965936, 0.974153561246036, -0.282740857326553, -0.152810756202003] +// A = [1.0, -2.12979364760736134, 0.42996125885751674, 1.62132698199721426, -0.96669962900852902, 0.00121015844426781, 0.04400300696788968] +SOS_IIR_Filter A_weighting = { + /* gain: */ sos_t(0.169994948147430), + /* sos: */ { // Second-Order Sections {b1, b2, -a1, -a2} + { sos_t(-2.00026996133106), sos_t(+1.00027056142719), + sos_t(-1.060868438509278), sos_t(-0.163987445885926) }, + { sos_t(+4.35912384203144), sos_t(+3.09120265783884), + sos_t(+1.208419926363593), sos_t(-0.273166998428332) }, + { sos_t(-0.70930303489759), sos_t(-0.29071868393580), + sos_t(+1.982242159753048), sos_t(-0.982298594928989) } } +}; + +//// +//// C-weighting IIR Filter, Fs = 48KHz +//// Designed by invfreqz curve-fitting, see respective .m file +//// B = [-0.49164716933714026, 0.14844753846498662, 0.74117815661529129, -0.03281878334039314, -0.29709276192593875, -0.06442545322197900, -0.00364152725482682] +//// A = [1.0, -1.0325358998928318, -0.9524000181023488, 0.8936404694728326 0.2256286147169398 -0.1499917107550188, 0.0156718181681081] +//SOS_IIR_Filter C_weighting = { +// gain: -0.491647169337140, +// sos: { +// { +1.4604385758204708, +0.5275070373815286, +1.9946144559930252, -0.9946217070140883 }, +// { +0.2376222404939509, +0.0140411206016894, -1.3396585608422749, -0.4421457807694559 }, +// { -2.0000000000000000, +1.0000000000000000, +0.3775800047420818, -0.0356365756680430 } } +//}; |