lasp/cpp_src/dsp/lasp_biquadbank.cpp

193 lines
4.8 KiB
C++

/* #define DEBUGTRACE_ENABLED */
#include "lasp_biquadbank.h"
#include "debugtrace.hpp"
#include "lasp_thread.h"
#include <cassert>
#include <vector>
using std::cerr;
using std::endl;
using rte = std::runtime_error;
using lock = std::scoped_lock<std::mutex>;
SeriesBiquad::SeriesBiquad(const vd &filter_coefs) {
DEBUGTRACE_ENTER;
if (filter_coefs.n_cols != 1) {
throw rte("Expected filter coefficients for a single SeriesBiquad as a "
"single column with length 6 x n_filters");
}
if (filter_coefs.n_rows % 6 != 0) {
cerr << "Number of rows given: " << filter_coefs.n_rows << endl;
throw rte("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)) {
std::cerr << "Read row: " << sos.row(3) << endl;
throw rte(
"Filter coefficients should have fourth element (a0) equal to 1.0");
}
}
SeriesBiquad SeriesBiquad::firstOrderHighPass(const d fs, const d cuton_Hz) {
if (fs <= 0) {
throw rte("Invalid sampling frequency: " + std::to_string(fs) + " [Hz]");
}
if (cuton_Hz <= 0) {
throw rte("Invalid cuton frequency: " + std::to_string(cuton_Hz) + " [Hz]");
}
if (cuton_Hz >= 0.98 * fs / 2) {
throw rte(
"Invalid cuton frequency. We limit this to 0.98* fs / 2. Given value" +
std::to_string(cuton_Hz) + " [Hz]");
}
const d tau = 1 / (2 * arma::datum::pi * cuton_Hz);
const d facnum = 2 * fs * tau / (1 + 2 * fs * tau);
const d facden = (1 - 2 * fs * tau) / (1 + 2 * fs * tau);
vd coefs(6);
// b0
coefs(0) = facnum;
// b1
coefs(1) = -facnum;
// b2
coefs(2) = 0;
// a0
coefs(3) = 1;
// a1
coefs(4) = facden;
// a2
coefs(5) = 0;
return SeriesBiquad(coefs);
}
std::unique_ptr<Filter> SeriesBiquad::clone() const {
// sos.as_col() concatenates all columns, exactly what we want.
return std::make_unique<SeriesBiquad>(sos.as_col());
}
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(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);
for (us sample = 0; sample < inout.size(); sample++) {
d w0 = inout(sample) - a1 * w1 - a2 * w2;
d yn = b0 * w0 + b1 * w1 + b2 * w2;
w2 = w1;
w1 = w0;
inout(sample) = yn;
}
state(0, filterno) = w1;
state(1, filterno) = w2;
}
}
BiquadBank::BiquadBank(const dmat &filters, const vd *gains) {
DEBUGTRACE_ENTER;
/**
* @brief Make sure the pool is created once, such that all threads are ready
* for use.
*/
lock lck(_mtx);
for (us i = 0; i < filters.n_cols; i++) {
_filters.emplace_back(filters.col(i));
}
if (gains != nullptr) {
setGains(*gains);
} else {
_gains = vd(_filters.size(), arma::fill::ones);
}
}
void BiquadBank::setGains(const vd &gains) {
DEBUGTRACE_ENTER;
lock lck(_mtx);
const us nfilters = _filters.size();
if (gains.size() != nfilters) {
throw rte("Invalid number of gain values given.");
}
_gains = gains;
}
void BiquadBank::filter(vd &inout) {
lock lck(_mtx);
std::vector<std::future<vd>> futs;
#if 1
vd inout_cpy = inout;
for (us i = 0; i < _filters.size(); i++) {
futs.emplace_back(_pool.submit(
[&](vd inout, us i) {
_filters[i].filter(inout);
return inout;
}, // Launch a task to filter.
inout_cpy, i // Column i as argument to the lambda function above.
));
}
// Zero-out in-out and sum-up the filtered values
inout.zeros();
for (us i = 0; i < _filters.size(); i++) {
inout += futs[i].get() * _gains[i];
}
#else
/// Testing, unthreaded version
vd inout_cpy = inout;
inout.zeros();
for (us i = 0; i < _filters.size(); i++) {
vd cpy_again = inout_cpy;
_filters[i].filter(cpy_again);
inout += cpy_again * _gains(i);
}
#endif
}
void BiquadBank::reset() {
DEBUGTRACE_ENTER;
lock lck(_mtx);
for (auto &f : _filters) {
f.reset();
}
}
std::unique_ptr<Filter> BiquadBank::clone() const {
lock lck(_mtx);
return std::make_unique<BiquadBank>(_filters, _gains);
}