128 lines
3.2 KiB
C++
128 lines
3.2 KiB
C++
#define DEBUGTRACE_ENABLED
|
|
#include "lasp_biquadbank.h"
|
|
#include "debugtrace.hpp"
|
|
#include "lasp_thread.h"
|
|
#include <vector>
|
|
|
|
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) {
|
|
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)) {
|
|
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(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.
|
|
*/
|
|
getPool();
|
|
|
|
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;
|
|
const us nfilters = _filters.size();
|
|
if (gains.size() != nfilters) {
|
|
throw runtime_error("Invalid number of gain values given.");
|
|
}
|
|
_gains = gains;
|
|
}
|
|
|
|
void BiquadBank::filter(vd &inout) {
|
|
|
|
dmat res(inout.n_rows, _filters.size());
|
|
std::vector<std::future<vd>> futs;
|
|
|
|
auto &pool = getPool();
|
|
for (us i = 0; i < res.n_cols; i++) {
|
|
futs.emplace_back(pool.submit(
|
|
[&](us i) {
|
|
// Copy column
|
|
vd col = inout;
|
|
_filters[i].filter(col);
|
|
return col;
|
|
}, // Launch a task to filter.
|
|
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 < res.n_cols; i++) {
|
|
inout += futs[i].get() * _gains[i];
|
|
}
|
|
}
|
|
void BiquadBank::reset() {
|
|
DEBUGTRACE_ENTER;
|
|
for (auto &f : _filters) {
|
|
f.reset();
|
|
}
|
|
}
|