From f8e8ab422bdee2f2ac6ab740e0034c9979b0b514 Mon Sep 17 00:00:00 2001 From: "J.A. de Jong - Redu-Sone B.V., ASCEE V.O.F" Date: Tue, 16 Aug 2022 21:22:35 +0200 Subject: [PATCH] SLM seems to be working. Needs proper testing. Not yet fully coupled to Python code --- src/lasp/CMakeLists.txt | 1 + src/lasp/dsp/lasp_biquadbank.cpp | 63 +++++--- src/lasp/dsp/lasp_biquadbank.h | 8 +- src/lasp/dsp/lasp_filter.h | 4 + src/lasp/dsp/lasp_slm.cpp | 221 ++++++++++++++++++++++++++ src/lasp/dsp/lasp_slm.h | 130 +++++++++++++++ src/lasp/pybind11/lasp_dsp_pybind.cpp | 32 +++- 7 files changed, 431 insertions(+), 28 deletions(-) create mode 100644 src/lasp/dsp/lasp_slm.cpp create mode 100644 src/lasp/dsp/lasp_slm.h diff --git a/src/lasp/CMakeLists.txt b/src/lasp/CMakeLists.txt index 8878ec5..6f0b612 100644 --- a/src/lasp/CMakeLists.txt +++ b/src/lasp/CMakeLists.txt @@ -6,6 +6,7 @@ add_definitions(-DARMA_DONT_USE_WRAPPER) configure_file(lasp_config.h.in lasp_config.h) include_directories(${CMAKE_CURRENT_BINARY_DIR}) include_directories(SYSTEM ../../third_party/armadillo-code/include) +include_directories(SYSTEM ../../third_party/carma/include) include_directories(../../third_party/DebugTrace-cpp/include) include_directories(../../third_party/lockfreeThreadsafe/include) include_directories(../../third_party/gsl-lite/include) diff --git a/src/lasp/dsp/lasp_biquadbank.cpp b/src/lasp/dsp/lasp_biquadbank.cpp index 8d33d76..f3193d8 100644 --- a/src/lasp/dsp/lasp_biquadbank.cpp +++ b/src/lasp/dsp/lasp_biquadbank.cpp @@ -1,42 +1,56 @@ -#include #define DEBUGTRACE_ENABLED -#include "debugtrace.hpp" #include "lasp_biquadbank.h" +#include "debugtrace.hpp" #include "lasp_thread.h" +#include +using std::cerr; +using std::endl; using std::runtime_error; SeriesBiquad::SeriesBiquad(const vd &filter_coefs) { + DEBUGTRACE_ENTER; if (filter_coefs.n_rows % 6 != 0) { - throw runtime_error("filter_coefs should be multiple of 6"); + cerr << "Number of rows given: " << filter_coefs.n_rows << endl; + throw runtime_error("filter_coefs should be multiple of 6, given: " + + std::to_string(filter_coefs.n_rows)); } us nfilters = filter_coefs.n_rows / 6; + /// Initialize state to zero + state = dmat(2, nfilters, arma::fill::zeros); + sos.resize(6, nfilters); for (us i = 0; i < nfilters; i++) { sos.col(i) = filter_coefs.subvec(6 * i, 6 * (i + 1) - 1); - - /// Check if third row in this matrix equals unity. - if (!arma::approx_equal(sos.row(3), - arma::rowvec(nfilters, arma::fill::ones), "absdiff", - 1e-9)) { - throw std::runtime_error( - "Filter coefficients should have fourth element (a0) equal to 1.0"); - } } - /// Initialize state to zero - state = dmat(2, nfilters, arma::fill::zeros); + + /// Check if third row in this matrix equals unity. + if (!arma::approx_equal(sos.row(3), arma::rowvec(nfilters, arma::fill::ones), + "absdiff", 1e-9)) { + std::cerr << "Read row: " << sos.row(3) << endl; + + throw std::runtime_error( + "Filter coefficients should have fourth element (a0) equal to 1.0"); + } +} +void SeriesBiquad::reset() { + DEBUGTRACE_ENTER; + state.zeros(); } void SeriesBiquad::filter(vd &inout) { + + DEBUGTRACE_ENTER; + /// Implementation is based on Proakis & Manolakis - Digital Signal /// Processing, Fourth Edition, p. 550 for (us filterno = 0; filterno < sos.n_cols; filterno++) { - d b0 = sos(filterno, 0); - d b1 = sos(filterno, 1); - d b2 = sos(filterno, 2); - d a1 = sos(filterno, 4); - d a2 = sos(filterno, 5); + d b0 = sos(0, filterno); + d b1 = sos(1, filterno); + d b2 = sos(2, filterno); + d a1 = sos(4, filterno); + d a2 = sos(5, filterno); d w1 = state(0, filterno); d w2 = state(1, filterno); @@ -60,7 +74,7 @@ BiquadBank::BiquadBank(const dmat &filters, const vd *gains) { * @brief Make sure the pool is created once, such that all threads are ready * for use. */ - auto& pool = getPool(); + getPool(); for (us i = 0; i < filters.n_cols; i++) { _filters.emplace_back(filters.col(i)); @@ -86,10 +100,9 @@ void BiquadBank::filter(vd &inout) { dmat res(inout.n_rows, _filters.size()); std::vector> futs; - auto& pool = getPool(); + auto &pool = getPool(); for (us i = 0; i < res.n_cols; i++) { - futs.emplace_back( - pool.submit( + futs.emplace_back(pool.submit( [&](us i) { // Copy column vd col = inout; @@ -106,3 +119,9 @@ void BiquadBank::filter(vd &inout) { inout += futs[i].get() * _gains[i]; } } +void BiquadBank::reset() { + DEBUGTRACE_ENTER; + for (auto &f : _filters) { + f.reset(); + } +} diff --git a/src/lasp/dsp/lasp_biquadbank.h b/src/lasp/dsp/lasp_biquadbank.h index efdba4e..a2cfbee 100644 --- a/src/lasp/dsp/lasp_biquadbank.h +++ b/src/lasp/dsp/lasp_biquadbank.h @@ -1,3 +1,4 @@ +#pragma once #include "lasp_filter.h" /** @@ -29,8 +30,9 @@ public: */ SeriesBiquad(const vd &filter_coefs); - virtual void filter(vd &inout) override; + virtual void filter(vd &inout) override final; virtual ~SeriesBiquad() override {} + void reset() override final; }; /** @@ -67,6 +69,8 @@ public: */ us nfilters() const {return _filters.size();} - virtual void filter(vd &inout) override; + virtual void filter(vd &inout) override final; + + void reset() override final; }; diff --git a/src/lasp/dsp/lasp_filter.h b/src/lasp/dsp/lasp_filter.h index e59e739..e895643 100644 --- a/src/lasp/dsp/lasp_filter.h +++ b/src/lasp/dsp/lasp_filter.h @@ -15,6 +15,10 @@ public: */ virtual void filter(vd &inout) = 0; virtual ~Filter() = 0; + /** + * @brief Reset filter state to 0 (history was all-zero). + */ + virtual void reset() = 0; }; diff --git a/src/lasp/dsp/lasp_slm.cpp b/src/lasp/dsp/lasp_slm.cpp new file mode 100644 index 0000000..3a630ed --- /dev/null +++ b/src/lasp/dsp/lasp_slm.cpp @@ -0,0 +1,221 @@ +#define DEBUGTRACE_ENABLED +#include "lasp_slm.h" +#include "debugtrace.hpp" +#include "lasp_thread.h" +#include +#include +#include +#include + +using std::cerr; +using std::endl; +using std::runtime_error; +using std::unique_ptr; + +SLM::SLM(const d fs, const d Lref, const us downsampling_fac, const d tau, + std::unique_ptr pre_filter, + std::vector> bandpass) + : _pre_filter(std::move(pre_filter)), _bandpass(std::move(bandpass)), + _alpha(exp(-1 / (fs * tau))), + _sp_storage(_bandpass.size(), arma::fill::zeros), // Storage for + // components of + // single pole low pass + // filter + Lref(Lref), // Reference level + downsampling_fac(downsampling_fac), + + // Initalize mean square + Pm(_bandpass.size(), arma::fill::zeros), + + // Initalize max + Pmax(_bandpass.size(), arma::fill::zeros), + + // Initalize peak + Ppeak(_bandpass.size(), arma::fill::zeros) + +{ + DEBUGTRACE_ENTER; + + // Make sure thread pool is running + getPool(); + + if (Lref <= 0) { + throw runtime_error("Invalid reference level"); + } + if (tau <= 0) { + throw runtime_error("Invalid time constant for Single pole lowpass filter"); + } + if (fs <= 0) { + throw runtime_error("Invalid sampling frequency"); + } +} +SLM::~SLM() {} + +/** + * @brief Create set bandpass filters from filter coefficients + * + * @param coefs + * + * @return + */ +std::vector> createBandPass(const dmat &coefs) { + DEBUGTRACE_ENTER; + std::vector> bf; + for (us colno = 0; colno < coefs.n_cols; colno++) { + bf.emplace_back(std::make_unique(coefs.col(colno))); + } + return bf; +} + +SLM SLM::fromBiquads(const d fs, const d Lref, const us downsampling_fac, + const d tau, const vd &pre_filter_coefs, + const dmat &bandpass_coefs) { + DEBUGTRACE_ENTER; + + return SLM(fs, Lref, downsampling_fac, tau, + std::make_unique(pre_filter_coefs), + createBandPass(bandpass_coefs)); +} +SLM SLM::fromBiquads(const d fs, const d Lref, const us downsampling_fac, + const d tau, const dmat &bandpass_coefs) { + + DEBUGTRACE_ENTER; + + return SLM(fs, Lref, downsampling_fac, tau, + nullptr, // Pre-filter + createBandPass(bandpass_coefs) // Bandpass coefficients + ); +} + +vd SLM::run_single(const us i, d *_work, const us size) { + + vd work(_work, // Aux Memory pointer + size, // Number of elements + false, // Copy aux mem + true // Stric, means we keep being bound to the memory + ); + + // Filter input in-place + _bandpass[i]->filter(work); + + /* cerr << "Filter done" << endl; */ + + // Square input --> Signal powers + /* work.transform([](d j) { return j * j; }); */ + work %= work; + + // Compute peak level, that is before single-pole low pass filter + Ppeak(i) = std::max(Ppeak(i), arma::max(work)); + + // Create copy of N, as we run this in multiple threads. + us N_local = N; + DEBUGTRACE_PRINT(N); + + // Obtain storage of single_pole low pass filter + d cur_storage = _sp_storage(i); + + for (us j = 0; j < work.n_rows; j++) { + // Update mean square of signal, work is here still signal power + Pm(i) = (Pm(i) * static_cast(N_local) + work(j)) / + (static_cast(N_local) + 1); + + N_local++; + + cur_storage = _alpha * cur_storage + (1 - _alpha) * work(j); + + // Now work is single-pole lowpassed signal power + work(j) = cur_storage; + } + + // And update storage of low-pass filter + _sp_storage(i) = cur_storage; + + Pmax(i) = std::max(Pmax(i), arma::max(work)); + + // Convert to levels in dB + /* work.transform([&](d val) { */ + /* return 10 * log10((val */ + + /* + arma::datum::eps // Add a bit of machine epsilon to */ + /* // the values to not compute -inf. */ + /* ) / */ + /* (Lref * Lref)); */ + /* }); */ + work = 10*arma::log10((work+arma::datum::eps)/(Lref*Lref)); + + return work; +} + +dmat SLM::run(const vd &input_orig) { + + DEBUGTRACE_ENTER; + vd input = input_orig; + + // _pre_filter filters in-place + if (_pre_filter) { + _pre_filter->filter(input); + } + + std::vector> futs; + auto &pool = getPool(); + + // Fan out over multiple threads, as it is typically a heavy load + dmat res(input.n_rows, _bandpass.size()); + + // Perform operations in-place. + for (us i = 0; i < _bandpass.size(); i++) { + res.col(i) = input; + + /// It is not possible to forward a vector of values from this array in a + /// sensible way to a different thread. We therefore break it down to + /// good-old pointers and values. + futs.emplace_back( + pool.submit(&SLM::run_single, this, i, res.colptr(i), res.n_rows)); + } + + /* DEBUGTRACE_PRINT(_bandpass.size()); */ + /* DEBUGTRACE_PRINT(res.n_cols); */ + /* DEBUGTRACE_PRINT(res.n_rows); */ + /* DEBUGTRACE_PRINT(futs.size()); */ + + // Wait for all threads to complete + for (us i = 0; i < _bandpass.size(); i++) { + futs[i].get(); + } + + // Update the total number of samples harvested so far. NOTE: *This should be + // done AFTER the threads are done!!!* + N += input.n_rows; + + // Downsample, if applicable + if (downsampling_fac > 1) { + dmat res_ds; + us rowno = 0; + while (cur_offset < res.n_rows) { + res_ds.insert_rows(rowno, res.row(cur_offset)); + rowno++; + + cur_offset += downsampling_fac; + } + cur_offset -= res.n_rows; + // Instead, return a downsampled version + return res_ds; + } + + return res; +} +void SLM::reset() { + Pm.zeros(); + Pmax.zeros(); + Ppeak.zeros(); + for (auto &f : _bandpass) { + f.reset(); + } + if (_pre_filter) { + _pre_filter->reset(); + } + _sp_storage.zeros(); + cur_offset = 0; + + N = 0; +} diff --git a/src/lasp/dsp/lasp_slm.h b/src/lasp/dsp/lasp_slm.h new file mode 100644 index 0000000..2a57ec3 --- /dev/null +++ b/src/lasp/dsp/lasp_slm.h @@ -0,0 +1,130 @@ +#pragma once +#include "lasp_biquadbank.h" +#include "lasp_filter.h" +#include +#include + +/** + * @brief Sound Level Meter implementation that gives a result for each + * channel. A channel is the result of a filtered signal + */ +class SLM { + /** + * @brief A, C or Z weighting, depending on the pre-filter installed. + */ + std::unique_ptr _pre_filter; + + /** + * @brief Bandpass filters for each channel + */ + std::vector> _bandpass; + /** + * @brief Storage for the single-pole low-pass filter coefficient based on + * the Fast / Slow time constant. < 0 means the filter is disabled. + */ + d _alpha = -1; + vd _sp_storage; + + d Lref; /// Reference value for computing decibels + us downsampling_fac; /// Every x'th sample is returned. + us cur_offset = 0; /// Storage for offset point in input arrays + /// +public: + /** + * @brief Public storage for the mean of the square of the signal. + */ + vd Pm; + /** + * @brief Public storage for the maximum signal power, after single pole + * low-pass filter. + */ + vd Pmax; /// Storage for maximum computed signal power so far. + /** + * @brief Public storage for the peak signal power, before single pole + * low-pass filter. + */ + vd Ppeak; + + us N = 0; /// Counter for the number of time samples counted that came + /// in; + + /** + * @brief Initialize a Sound Level Meter + * + * @param fs Sampling frequency [Hz] + * @param Lref Level reference, used to scale to proper decibel units (dB + * SPL / dBV, etc) + * @param downsampling_fac Every 1/downsampling_fac value is returned from + * compute() + * @param tau Time consant of level meter + * @param pre_filter The pre-filter (Typically an A/C frequency weighting + * filter) + * @param bandpass The parallel set of bandpass filters. + */ + SLM(const d fs, const d Lref, const us downsampling_fac, const d tau, + std::unique_ptr pre_filter, + std::vector> bandpass); + + /** + * @brief Convenience function to create a Sound Level meter from Biquad + * filters only. + * + * @param fs Sampling frequency [Hz] + * @param Lref Level reference, used to scale to proper decibel units (dB + * SPL / dBV, etc) + * @param downsampling_fac Every 1/downsampling_fac value is returned from + * compute() + * @param tau Time consant of level meter + * @param pre_filter_coefs Biquad filter coefficients for pre-filter + * @param bandpass_coefs Biquad filter coeffiecients for bandpass filter + * + * @return Sound Level Meter object + */ + static SLM fromBiquads(const d fs, const d Lref, const us downsampling_fac, + const d tau, const vd &pre_filter_coefs, + const dmat &bandpass_coefs); + /** + * @brief Convenience function to create a Sound Level meter from Biquad + * filters only. No pre-filter, only bandpass. + * + * @param fs Sampling frequency [Hz] + * @param Lref Level reference, used to scale to proper decibel units (dB + * SPL / dBV, etc) + * @param downsampling_fac Every 1/downsampling_fac value is returned from + * compute() + * @param tau Time consant of level meter + * @param bandpass_coefs Biquad filter coeffiecients for bandpass filter + * + * @return Sound Level Meter object + */ + static SLM fromBiquads(const d fs, const d Lref, const us downsampling_fac, + const d tau, const dmat &bandpass_coefs); + + ~SLM(); + + /** + * @brief Reset state related to samples acquired. All filters reset to zero. + * Start again from no history. + */ + void reset(); + + SLM(const SLM &o) = delete; + SLM &operator=(const SLM &o) = delete; + SLM(SLM &&o) = default; + + /** + * @brief Run the sound level meter on given input data. Return downsampled + * level data for each filterbank channel. + * + * @param input Raw input data + * + * @return Filtered level data. + */ + dmat run(const vd &input); + vd Lpeak() const { return 10*arma::log10(Ppeak/Lref);}; + vd Leq() const { return 10*arma::log10(Pm/Lref);}; + vd Lmax() const { return 10*arma::log10(Pmax/Lref);}; + +private: + vd run_single(const us i,d* inout,const us size); +}; diff --git a/src/lasp/pybind11/lasp_dsp_pybind.cpp b/src/lasp/pybind11/lasp_dsp_pybind.cpp index 60fa2cd..3ff4002 100644 --- a/src/lasp/pybind11/lasp_dsp_pybind.cpp +++ b/src/lasp/pybind11/lasp_dsp_pybind.cpp @@ -1,13 +1,16 @@ -#include "lasp_window.h" +#include "carma" +#include "lasp_biquadbank.h" #include "lasp_siggen.h" #include "lasp_siggen_impl.h" +#include "lasp_slm.h" +#include "lasp_window.h" #include #include using std::cerr; namespace py = pybind11; -void init_dsp(py::module& m) { +void init_dsp(py::module &m) { py::class_ w(m, "Window"); @@ -20,10 +23,31 @@ void init_dsp(py::module& m) { .export_values(); py::class_> siggen(m, "Siggen"); - siggen.def("setLevel", &Siggen::setLevel, "Set the level of the signal generator"); - + siggen.def("setLevel", &Siggen::setLevel, + "Set the level of the signal generator"); py::class_> sw(m, "Sine", siggen); sw.def(py::init()); + py::class_ sbq(m, "SeriesBiquad"); + sbq.def(py::init()); + sbq.def("filter", [](SeriesBiquad &s, const vd &input) { + vd res = input; + s.filter(res); + return res; + }); + + py::class_ slm(m, "SLM"); + slm.def_static( + "fromBiquads", + py::overload_cast( + &SLM::fromBiquads)); + slm.def_static( + "fromBiquads", + py::overload_cast(&SLM::fromBiquads)); + slm.def("run", &SLM::run); + slm.def_readonly("Pm", &SLM::Pm); + slm.def_readonly("Pmax", &SLM::Pmax); + slm.def_readonly("Ppeak", &SLM::Ppeak); }