lasp/cpp_src/dsp/lasp_slm.cpp

208 lines
5.2 KiB
C++
Raw Normal View History

/* #define DEBUGTRACE_ENABLED */
#include "debugtrace.hpp"
#include "lasp_slm.h"
#include "lasp_thread.h"
#include <algorithm>
#include <cmath>
#include <future>
#include <memory>
using std::cerr;
using std::endl;
using rte = 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<Filter> pre_filter,
std::vector<std::unique_ptr<Filter>> 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
Lrefsq(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;
DEBUGTRACE_PRINT(_alpha);
if (Lref <= 0) {
throw rte("Invalid reference level");
}
if (tau < 0) {
throw rte("Invalid time constant for Single pole lowpass filter");
}
if (fs <= 0) {
throw rte("Invalid sampling frequency");
}
}
SLM::~SLM() {}
/**
* @brief Create set bandpass filters from filter coefficients
*
* @param coefs
*
* @return
*/
std::vector<unique_ptr<Filter>> createBandPass(const dmat &coefs) {
DEBUGTRACE_ENTER;
std::vector<unique_ptr<Filter>> bf;
for (us colno = 0; colno < coefs.n_cols; colno++) {
bf.emplace_back(std::make_unique<SeriesBiquad>(coefs.col(colno)));
}
return bf;
}
us SLM::suggestedDownSamplingFac(const d fs, const d tau) {
if (fs <= 0)
throw rte("Invalid sampling frequency");
// A reasonable 'framerate' for the sound level meter, based on the
// filtering time constant.
if (tau > 0) {
d fs_slm = 10 / tau;
if (fs_slm < 30) {
fs_slm = 30;
}
return std::max((us)1, static_cast<us>(fs / fs_slm));
} else {
return 1;
}
}
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<SeriesBiquad>(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(vd work, const us i) {
// 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<d>(N_local) + work(j)) /
(static_cast<d>(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 = 10 * arma::log10((work + arma::datum::eps) / Lrefsq);
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);
}
// Fan out over multiple threads, as it is typically a heavy load
dmat res(input.n_rows, _bandpass.size());
// Perform operations in-place.
#pragma omp parallel for
for (us i = 0; i < _bandpass.size(); i++) {
res.col(i) = run_single(input, i);
/* DEBUGTRACE_PRINT(_bandpass.size()); */
/* DEBUGTRACE_PRINT(res.n_cols); */
/* DEBUGTRACE_PRINT(res.n_rows); */
/* DEBUGTRACE_PRINT(futs.size()); */
// 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;
}