You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
129 lines
4.5 KiB
C++
129 lines
4.5 KiB
C++
/*
|
|
* 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 } }
|
|
//};
|