fix qfplib dependence; clean up filter()

main
Clyne 5 months ago
parent eea21d422b
commit 1fad862eea
Signed by: clyne
GPG Key ID: 3267C8EBF3F9AFC7

@ -15,7 +15,7 @@ endif
# C++ specific options here (added to USE_OPT). # C++ specific options here (added to USE_OPT).
ifeq ($(USE_CPPOPT),) ifeq ($(USE_CPPOPT),)
USE_CPPOPT = -std=c++23 -fno-exceptions -fno-rtti USE_CPPOPT = -std=c++23 -fno-exceptions -fno-rtti -fno-threadsafe-statics
endif endif
# Enable this if you want the linker to remove unused code and data. # Enable this if you want the linker to remove unused code and data.

@ -9,9 +9,9 @@
static constexpr auto& WEIGHTING = A_weighting; static constexpr auto& WEIGHTING = A_weighting;
static constexpr auto& MIC_EQUALIZER = SPH0645LM4H_B_RB; static constexpr auto& MIC_EQUALIZER = SPH0645LM4H_B_RB;
static constexpr float MIC_OFFSET_DB ( 0.f); // Linear offset static constexpr sos_t MIC_OFFSET_DB ( 0.f); // Linear offset
static constexpr float MIC_SENSITIVITY (-26.f); // dBFS value expected at MIC_REF_DB static constexpr sos_t MIC_SENSITIVITY (-26.f); // dBFS value expected at MIC_REF_DB
static constexpr float MIC_REF_DB ( 94.f); // dB where sensitivity is specified static constexpr sos_t MIC_REF_DB ( 94.f); // dB where sensitivity is specified
static constexpr sos_t MIC_OVERLOAD_DB (120.f); // dB - Acoustic overload point static constexpr sos_t MIC_OVERLOAD_DB (120.f); // dB - Acoustic overload point
static constexpr sos_t MIC_NOISE_DB ( 29.f); // dB - Noise floor static constexpr sos_t MIC_NOISE_DB ( 29.f); // dB - Noise floor
static constexpr auto MIC_BITS = 18u; static constexpr auto MIC_BITS = 18u;
@ -21,8 +21,8 @@ static constexpr unsigned I2S_BUFSIZ = 1024;
static constexpr unsigned I2S_STRIDE = 16; static constexpr unsigned I2S_STRIDE = 16;
// Calculate reference amplitude value at compile time // Calculate reference amplitude value at compile time
static const auto MIC_REF_AMPL = qfp_fpow(10.f, MIC_SENSITIVITY / 20) * static const auto MIC_REF_AMPL = sos_t((1 << (MIC_BITS - 1)) - 1) *
((1 << (MIC_BITS - 1)) - 1); qfp_fpow(10.f, MIC_SENSITIVITY / 20.f);
static SEMAPHORE_DECL(i2sReady, 0); static SEMAPHORE_DECL(i2sReady, 0);
static THD_WORKING_AREA(waThread1, 128); static THD_WORKING_AREA(waThread1, 128);
@ -83,13 +83,13 @@ THD_FUNCTION(Thread1, arg)
palSetPad(GPIOB, 7); palSetPad(GPIOB, 7);
chSemWait(&i2sReady); chSemWait(&i2sReady);
const auto Leq_RMS = qfp_fsqrt((float)Leq_sum_sqr / Leq_samples); const sos_t Leq_RMS = qfp_fsqrt(Leq_sum_sqr / qfp_uint2float(Leq_samples));
const auto Leq_dB = MIC_OFFSET_DB + MIC_REF_DB + 20 * const sos_t Leq_dB = MIC_OFFSET_DB + MIC_REF_DB + sos_t(20.f) *
qfp_flog10(Leq_RMS / MIC_REF_AMPL); qfp_flog10(Leq_RMS / MIC_REF_AMPL);
Leq_sum_sqr = sos_t(0.f); Leq_sum_sqr = sos_t(0.f);
Leq_samples = 0; Leq_samples = 0;
auto n = std::clamp(static_cast<int32_t>(Leq_dB), 0l, 999l); auto n = std::clamp(qfp_float2int(Leq_dB), 0, 999);
strbuf[2] = n % 10 + '0'; n /= 10; strbuf[2] = n % 10 + '0'; n /= 10;
strbuf[1] = n % 10 + '0'; n /= 10; strbuf[1] = n % 10 + '0'; n /= 10;
strbuf[0] = n ? n + '0' : ' '; strbuf[0] = n ? n + '0' : ' ';
@ -111,7 +111,7 @@ void i2sCallback(I2SDriver *i2s)
std::ranges::copy( std::ranges::copy(
std::views::counted(i2sBuffer.begin() + offset, halfsize / I2S_STRIDE) std::views::counted(i2sBuffer.begin() + offset, halfsize / I2S_STRIDE)
| std::ranges::views::stride(2) | std::ranges::views::stride(2)
| std::views::transform([](uint32_t s) { return sos_t(fixsample(s)); }), | std::views::transform([](uint32_t s) { return sos_t(qfp_int2float(fixsample(s))); }),
samples); samples);
auto samps = std::views::counted(samples, halfsize / (2 * I2S_STRIDE)); auto samps = std::views::counted(samples, halfsize / (2 * I2S_STRIDE));

@ -32,34 +32,36 @@ extern "C" {
float qfp_fpow(float b, float e) float qfp_fpow(float b, float e)
{ {
return qfp_fexp(e * qfp_fln(b)); return qfp_fexp(qfp_fmul(e, qfp_fln(b)));
} }
float qfp_flog10(float x) float qfp_flog10(float x)
{ {
return qfp_fln(x) / qfp_fln(10); static const auto ln10 = qfp_fln(10.f);
return qfp_fdiv(qfp_fln(x), ln10);
} }
struct sos_t class sos_t
{ {
float v; float v;
public:
constexpr sos_t(float v_ = 0.f): v(v_) {} constexpr sos_t(float v_ = 0.f): v(v_) {}
sos_t operator+(const sos_t& o) const noexcept { sos_t operator+(auto x) const noexcept {
return qfp_fadd(v, o.v); return qfp_fadd(v, x);
} }
sos_t operator-(const sos_t& o) const noexcept { sos_t operator-(const sos_t& o) const noexcept {
return qfp_fsub(v, o.v); return qfp_fsub(v, o.v);
} }
sos_t operator*(const sos_t& o) const noexcept { sos_t operator*(auto x) const noexcept {
return qfp_fmul(v, o.v); return qfp_fmul(v, x);
} }
sos_t operator/(const sos_t& o) const noexcept { sos_t operator/(auto x) const noexcept {
return qfp_fdiv(v, o.v); return qfp_fdiv(v, x);
} }
sos_t& operator+=(const sos_t& o) noexcept { sos_t& operator+=(const sos_t& o) noexcept {
@ -100,17 +102,14 @@ struct SOS_IIR_Filter {
} }
void filter(auto samples, std::size_t n = N) { void filter(auto samples, std::size_t n = N) {
for ([[maybe_unused]] auto _ : std::views::zip_transform( for (auto [coeffs, ww] : std::views::zip(sos, w) | std::views::take(n)) {
[](auto samps, const auto& coeffs, auto& ww) { // Assumes a0 and b0 coefficients are one (1.0)
// Assumes a0 and b0 coefficients are one (1.0) for (auto& s : samples) {
for (auto& s : samps) { auto f6 = s + coeffs.a1 * ww.w0 + coeffs.a2 * ww.w1;
auto f6 = s + coeffs.a1 * ww.w0 + coeffs.a2 * ww.w1; s = f6 + coeffs.b1 * ww.w0 + coeffs.b2 * ww.w1;
s = f6 + coeffs.b1 * ww.w0 + coeffs.b2 * ww.w1; ww.w1 = std::exchange(ww.w0, f6);
ww.w1 = std::exchange(ww.w0, f6); }
} }
return 0;
},
std::views::repeat(samples, n), sos, w)) {}
} }
sos_t filter_sum_sqr(auto samples) { sos_t filter_sum_sqr(auto samples) {

Loading…
Cancel
Save