340 lines
9.6 KiB
C++
340 lines
9.6 KiB
C++
/** @file
|
|
* @author Edouard DUPIN
|
|
* @copyright 2011, Edouard DUPIN, all right reserved
|
|
* @license APACHE v2.0 (see license file)
|
|
*/
|
|
|
|
#include <drain/debug.h>
|
|
#include <drain/Equalizer.h>
|
|
#include <drain/Algo.h>
|
|
|
|
// see http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
|
|
// see http://www.earlevel.com/main/2013/10/13/biquad-calculator-v2/
|
|
|
|
drain::Equalizer::Equalizer() {
|
|
|
|
}
|
|
|
|
void drain::Equalizer::init() {
|
|
drain::Algo::init();
|
|
drain::Algo::m_type = "Equalizer";
|
|
m_supportedFormat.push_back(audio::format_int16);
|
|
m_supportedFormat.push_back(audio::format_float);
|
|
m_type = drain::filterType_none;
|
|
m_gain = 6;
|
|
m_frequencyCut = 1000;
|
|
m_qualityFactor = 0.707;
|
|
configureBiQuad();
|
|
// reset coefficients
|
|
m_a[0] = 1.0;
|
|
m_a[1] = 0.0;
|
|
m_a[2] = 0.0;
|
|
m_b[0] = 0.0;
|
|
m_b[1] = 0.0;
|
|
}
|
|
|
|
std11::shared_ptr<drain::Equalizer> drain::Equalizer::create() {
|
|
std11::shared_ptr<drain::Equalizer> tmp(new drain::Equalizer());
|
|
tmp->init();
|
|
return tmp;
|
|
}
|
|
|
|
drain::Equalizer::~Equalizer() {
|
|
|
|
}
|
|
|
|
void drain::Equalizer::configurationChange() {
|
|
drain::Algo::configurationChange();
|
|
// Resize the configuration ouput of algirithm
|
|
m_history.clear();
|
|
m_history.resize(getOutputFormat().getMap().size());
|
|
}
|
|
|
|
bool drain::Equalizer::process(std11::chrono::system_clock::time_point& _time,
|
|
void* _input,
|
|
size_t _inputNbChunk,
|
|
void*& _output,
|
|
size_t& _outputNbChunk) {
|
|
_outputNbChunk = _inputNbChunk;
|
|
_output = _input;
|
|
if (_input == nullptr) {
|
|
return false;
|
|
}
|
|
if (getOutputFormat().getFormat() == audio::format_float) {
|
|
for (size_t jjj=0; jjj<getOutputFormat().getMap().size(); ++jjj) {
|
|
// get pointer on data:
|
|
float* data = static_cast<float*>(_input);
|
|
// move to sample offset:
|
|
data += jjj;
|
|
for (size_t iii=0; iii<_inputNbChunk; ++iii) {
|
|
// process in float the biquad.
|
|
*data = processFloat(*data, m_history[jjj]);
|
|
// move to the sample on the same channel.
|
|
data += getOutputFormat().getMap().size();
|
|
}
|
|
}
|
|
} else if (getOutputFormat().getFormat() == audio::format_int16) {
|
|
for (size_t jjj=0; jjj<getOutputFormat().getMap().size(); ++jjj) {
|
|
// get pointer on data:
|
|
int16_t* data = static_cast<int16_t*>(_input);
|
|
// move to sample offset:
|
|
data += jjj;
|
|
for (size_t iii=0; iii<_inputNbChunk; ++iii) {
|
|
// process in float the biquad.
|
|
float out = processFloat(*data, m_history[jjj]);
|
|
// Limit output.
|
|
out = std::avg(-32768.0f, out, 32767.0f);
|
|
*data = static_cast<int16_t>(out);
|
|
// move to the sample on the same channel.
|
|
data += getOutputFormat().getMap().size();
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool drain::Equalizer::setParameter(const std::string& _parameter, const std::string& _value) {
|
|
//DRAIN_WARNING("set : " << _parameter << " " << _value);
|
|
if (_parameter == "type") {
|
|
if (_value == "none") {
|
|
m_type = drain::filterType_none;
|
|
} else if (_value == "LPF") {
|
|
m_type = drain::filterType_lowPass;
|
|
} else if (_value == "HPF") {
|
|
m_type = drain::filterType_highPass;
|
|
} else if (_value == "BPF") {
|
|
m_type = drain::filterType_bandPass;
|
|
} else if (_value == "NOTCH") {
|
|
m_type = drain::filterType_notch;
|
|
} else if (_value == "PeakingEQ") {
|
|
m_type = drain::filterType_peak;
|
|
} else if (_value == "LSH") {
|
|
m_type = drain::filterType_lowShelf;
|
|
} else if (_value == "HSH") {
|
|
m_type = drain::filterType_highShelf;
|
|
} else {
|
|
DRAIN_ERROR("Can not set equalizer type : " << _value);
|
|
return false;
|
|
}
|
|
configureBiQuad();
|
|
return true;
|
|
} else if (_parameter == "gain") {
|
|
m_gain = etk::string_to_double(_value);
|
|
configureBiQuad();
|
|
return true;
|
|
} else if (_parameter == "frequency") {
|
|
m_frequencyCut = etk::string_to_double(_value);
|
|
configureBiQuad();
|
|
return true;
|
|
} else if (_parameter == "quality") {
|
|
m_qualityFactor = etk::string_to_double(_value);
|
|
configureBiQuad();
|
|
return true;
|
|
} else if (_parameter == "reset") {
|
|
m_history.clear();
|
|
m_history.resize(getOutputFormat().getMap().size());
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
std::string drain::Equalizer::getParameter(const std::string& _parameter) const {
|
|
return "error";
|
|
}
|
|
|
|
std::string drain::Equalizer::getParameterProperty(const std::string& _parameter) const {
|
|
return "error";
|
|
}
|
|
|
|
float drain::Equalizer::processFloat(float _sample, drain::BGHistory& _history) {
|
|
float result;
|
|
// compute
|
|
result = m_a[0] * _sample
|
|
+ m_a[1] * _history.m_x[0]
|
|
+ m_a[2] * _history.m_x[1]
|
|
- m_b[0] * _history.m_y[0]
|
|
- m_b[1] * _history.m_y[1];
|
|
//update history of X
|
|
_history.m_x[1] = _history.m_x[0];
|
|
_history.m_x[0] = _sample;
|
|
//update history of Y
|
|
_history.m_y[1] = _history.m_y[0];
|
|
_history.m_y[0] = result;
|
|
return result;
|
|
}
|
|
|
|
bool drain::Equalizer::configureBiQuad() {
|
|
calcBiquad(m_type, m_frequencyCut, m_qualityFactor, m_gain);
|
|
return true;
|
|
}
|
|
|
|
void drain::Equalizer::calcBiquad(enum drain::filterType _type, double _frequencyCut, double _qualityFactor, double _gain) {
|
|
m_type = _type;
|
|
m_frequencyCut = _frequencyCut;
|
|
m_qualityFactor = _qualityFactor;
|
|
m_gain = _gain;
|
|
|
|
if (getOutputFormat().getFrequency() < 1) {
|
|
m_a[0] = 1.0;
|
|
m_a[1] = 0.0;
|
|
m_a[2] = 0.0;
|
|
m_b[0] = 0.0;
|
|
m_b[1] = 0.0;
|
|
return;
|
|
}
|
|
if (m_frequencyCut > getOutputFormat().getFrequency()/2) {
|
|
m_frequencyCut = getOutputFormat().getFrequency()/2;
|
|
} else if (m_frequencyCut < 0) {
|
|
m_frequencyCut = 0;
|
|
}
|
|
if (m_qualityFactor < 0.01) {
|
|
m_qualityFactor = 0.01;
|
|
}
|
|
switch (m_type) {
|
|
case filterType_lowPass:
|
|
case filterType_highPass:
|
|
case filterType_bandPass:
|
|
case filterType_notch:
|
|
// Quality : USE IT
|
|
// Gain : Not USE IT
|
|
break;
|
|
case filterType_peak:
|
|
// Quality : USE IT
|
|
// Gain : USE IT
|
|
break;
|
|
case filterType_lowShelf:
|
|
case filterType_highShelf:
|
|
// Quality : NOT USE IT
|
|
// Gain : USE IT
|
|
break;
|
|
default:
|
|
// Quality : USE IT
|
|
// Gain : USE IT
|
|
break;
|
|
}
|
|
double norm;
|
|
double V = std::pow(10.0, std::abs(m_gain) / 20.0);
|
|
double K = std::tan(M_PI * m_frequencyCut / getOutputFormat().getFrequency());
|
|
switch (m_type) {
|
|
case filterType_none:
|
|
m_a[0] = 1.0;
|
|
m_a[1] = 0.0;
|
|
m_a[2] = 0.0;
|
|
m_b[0] = 0.0;
|
|
m_b[1] = 0.0;
|
|
break;
|
|
case filterType_lowPass:
|
|
norm = 1 / (1 + K / m_qualityFactor + K * K);
|
|
m_a[0] = K * K * norm;
|
|
m_a[1] = 2 * m_a[0];
|
|
m_a[2] = m_a[0];
|
|
m_b[0] = 2 * (K * K - 1) * norm;
|
|
m_b[1] = (1 - K / m_qualityFactor + K * K) * norm;
|
|
break;
|
|
case filterType_highPass:
|
|
norm = 1 / (1 + K / m_qualityFactor + K * K);
|
|
m_a[0] = 1 * norm;
|
|
m_a[1] = -2 * m_a[0];
|
|
m_a[2] = m_a[0];
|
|
m_b[0] = 2 * (K * K - 1) * norm;
|
|
m_b[1] = (1 - K / m_qualityFactor + K * K) * norm;
|
|
break;
|
|
case filterType_bandPass:
|
|
norm = 1 / (1 + K / m_qualityFactor + K * K);
|
|
m_a[0] = K / m_qualityFactor * norm;
|
|
m_a[1] = 0;
|
|
m_a[2] = -m_a[0];
|
|
m_b[0] = 2 * (K * K - 1) * norm;
|
|
m_b[1] = (1 - K / m_qualityFactor + K * K) * norm;
|
|
break;
|
|
case filterType_notch:
|
|
norm = 1 / (1 + K / m_qualityFactor + K * K);
|
|
m_a[0] = (1 + K * K) * norm;
|
|
m_a[1] = 2 * (K * K - 1) * norm;
|
|
m_a[2] = m_a[0];
|
|
m_b[0] = m_a[1];
|
|
m_b[1] = (1 - K / m_qualityFactor + K * K) * norm;
|
|
break;
|
|
case filterType_peak:
|
|
if (m_gain >= 0) {
|
|
norm = 1 / (1 + 1/m_qualityFactor * K + K * K);
|
|
m_a[0] = (1 + V/m_qualityFactor * K + K * K) * norm;
|
|
m_a[1] = 2 * (K * K - 1) * norm;
|
|
m_a[2] = (1 - V/m_qualityFactor * K + K * K) * norm;
|
|
m_b[0] = m_a[1];
|
|
m_b[1] = (1 - 1/m_qualityFactor * K + K * K) * norm;
|
|
} else {
|
|
norm = 1 / (1 + V/m_qualityFactor * K + K * K);
|
|
m_a[0] = (1 + 1/m_qualityFactor * K + K * K) * norm;
|
|
m_a[1] = 2 * (K * K - 1) * norm;
|
|
m_a[2] = (1 - 1/m_qualityFactor * K + K * K) * norm;
|
|
m_b[0] = m_a[1];
|
|
m_b[1] = (1 - V/m_qualityFactor * K + K * K) * norm;
|
|
}
|
|
break;
|
|
case filterType_lowShelf:
|
|
if (m_gain >= 0) {
|
|
norm = 1 / (1 + M_SQRT2 * K + K * K);
|
|
m_a[0] = (1 + std::sqrt(2*V) * K + V * K * K) * norm;
|
|
m_a[1] = 2 * (V * K * K - 1) * norm;
|
|
m_a[2] = (1 - std::sqrt(2*V) * K + V * K * K) * norm;
|
|
m_b[0] = 2 * (K * K - 1) * norm;
|
|
m_b[1] = (1 - M_SQRT2 * K + K * K) * norm;
|
|
} else {
|
|
norm = 1 / (1 + std::sqrt(2*V) * K + V * K * K);
|
|
m_a[0] = (1 + M_SQRT2 * K + K * K) * norm;
|
|
m_a[1] = 2 * (K * K - 1) * norm;
|
|
m_a[2] = (1 - M_SQRT2 * K + K * K) * norm;
|
|
m_b[0] = 2 * (V * K * K - 1) * norm;
|
|
m_b[1] = (1 - std::sqrt(2*V) * K + V * K * K) * norm;
|
|
}
|
|
break;
|
|
case filterType_highShelf:
|
|
if (m_gain >= 0) {
|
|
norm = 1 / (1 + M_SQRT2 * K + K * K);
|
|
m_a[0] = (V + std::sqrt(2*V) * K + K * K) * norm;
|
|
m_a[1] = 2 * (K * K - V) * norm;
|
|
m_a[2] = (V - std::sqrt(2*V) * K + K * K) * norm;
|
|
m_b[0] = 2 * (K * K - 1) * norm;
|
|
m_b[1] = (1 - M_SQRT2 * K + K * K) * norm;
|
|
} else {
|
|
norm = 1 / (V + std::sqrt(2*V) * K + K * K);
|
|
m_a[0] = (1 + M_SQRT2 * K + K * K) * norm;
|
|
m_a[1] = 2 * (K * K - 1) * norm;
|
|
m_a[2] = (1 - M_SQRT2 * K + K * K) * norm;
|
|
m_b[0] = 2 * (K * K - V) * norm;
|
|
m_b[1] = (V - std::sqrt(2*V) * K + K * K) * norm;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
static const char* listValues[] = {
|
|
"none",
|
|
"low-pass",
|
|
"high-pass",
|
|
"band-pass",
|
|
"notch",
|
|
"peak",
|
|
"low-shelf",
|
|
"high-shelf"
|
|
};
|
|
static int32_t listValuesSize = sizeof(listValues)/sizeof(char*);
|
|
|
|
|
|
namespace etk {
|
|
template<> std::string to_string<enum drain::filterType>(const enum drain::filterType& _variable) {
|
|
return listValues[_variable];
|
|
}
|
|
template <> bool from_string<enum drain::filterType>(enum drain::filterType& _variableRet, const std::string& _value) {
|
|
for (int32_t iii=0; iii<listValuesSize; ++iii) {
|
|
if (_value == listValues[iii]) {
|
|
_variableRet = static_cast<enum drain::filterType>(iii);
|
|
return true;
|
|
}
|
|
}
|
|
_variableRet = drain::filterType_none;
|
|
return false;
|
|
}
|
|
}
|