[DEV] update equalizer

This commit is contained in:
Edouard DUPIN 2015-05-04 22:25:32 +02:00
parent 154d03e8bd
commit ab7f5ac771
5 changed files with 28 additions and 452 deletions

View File

@ -1,285 +0,0 @@
/** @file
* @author Edouard DUPIN
* @copyright 2011, Edouard DUPIN, all right reserved
* @license APACHE v2.0 (see license file)
*/
#include <audio/drain/debug.h>
#include <audio/drain/BiQuadFloat.h>
#include <cmath>
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 audio::drain::filterType>(const enum audio::drain::filterType& _variable) {
return listValues[_variable];
}
template <> bool from_string<enum audio::drain::filterType>(enum audio::drain::filterType& _variableRet, const std::string& _value) {
for (int32_t iii=0; iii<listValuesSize; ++iii) {
if (_value == listValues[iii]) {
_variableRet = static_cast<enum audio::drain::filterType>(iii);
return true;
}
}
_variableRet = audio::drain::filterType_none;
return false;
}
}
audio::drain::BiQuadFloat::BiQuadFloat() {
reset();
// 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;
}
float audio::drain::BiQuadFloat::processFloat(float _sample) {
float result;
// compute
result = m_a[0] * _sample
+ m_a[1] * m_x[0]
+ m_a[2] * m_x[1]
- m_b[0] * m_y[0]
- m_b[1] * m_y[1];
//update history of X
m_x[1] = m_x[0];
m_x[0] = _sample;
//update history of Y
m_y[1] = m_y[0];
m_y[0] = result;
return result;
}
void audio::drain::BiQuadFloat::processFloat(float* _input,
float* _output,
size_t _nbChunk,
int32_t _inputOffset,
int32_t _outputOffset) {
for (size_t iii=0; iii<_nbChunk; ++iii) {
// process in float the biquad.
*_output = processFloat(*_input);
// move to the sample on the same channel.
_input += _inputOffset;
_output += _outputOffset;
}
}
void audio::drain::BiQuadFloat::processInt16(int16_t* _input,
int16_t* _output,
size_t _nbChunk,
int32_t _inputOffset,
int32_t _outputOffset) {
for (size_t iii=0; iii<_nbChunk; ++iii) {
// process in float the biquad.
float out = processFloat(*_input);
// Limit output.
out = std::avg(-32768.0f, out, 32767.0f);
*_output = static_cast<int16_t>(out);
// move to the sample on the same channel.
_input += _inputOffset;
_output += _outputOffset;
}
}
void audio::drain::BiQuadFloat::setBiquad(enum audio::drain::filterType _type, double _frequencyCut, double _qualityFactor, double _gain, float _sampleRate) {
reset();
if (_sampleRate < 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 (_frequencyCut > _sampleRate/2) {
_frequencyCut = _sampleRate/2;
} else if (_frequencyCut < 0) {
_frequencyCut = 0;
}
if (_qualityFactor < 0.01) {
_qualityFactor = 0.01;
}
double norm;
double V = std11::pow(10.0, std11::abs(_gain) / 20.0);
double K = std11::tan(M_PI * _frequencyCut / _sampleRate);
switch (_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 / _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 / _qualityFactor + K * K) * norm;
break;
case filterType_highPass:
norm = 1 / (1 + K / _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 / _qualityFactor + K * K) * norm;
break;
case filterType_bandPass:
norm = 1 / (1 + K / _qualityFactor + K * K);
m_a[0] = K / _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 / _qualityFactor + K * K) * norm;
break;
case filterType_notch:
norm = 1 / (1 + K / _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 / _qualityFactor + K * K) * norm;
break;
case filterType_peak:
if (_gain >= 0) {
norm = 1 / (1 + 1/_qualityFactor * K + K * K);
m_a[0] = (1 + V/_qualityFactor * K + K * K) * norm;
m_a[1] = 2 * (K * K - 1) * norm;
m_a[2] = (1 - V/_qualityFactor * K + K * K) * norm;
m_b[0] = m_a[1];
m_b[1] = (1 - 1/_qualityFactor * K + K * K) * norm;
} else {
norm = 1 / (1 + V/_qualityFactor * K + K * K);
m_a[0] = (1 + 1/_qualityFactor * K + K * K) * norm;
m_a[1] = 2 * (K * K - 1) * norm;
m_a[2] = (1 - 1/_qualityFactor * K + K * K) * norm;
m_b[0] = m_a[1];
m_b[1] = (1 - V/_qualityFactor * K + K * K) * norm;
}
break;
case filterType_lowShelf:
if (_gain >= 0) {
norm = 1 / (1 + M_SQRT2 * K + K * K);
m_a[0] = (1 + std11::sqrt(2*V) * K + V * K * K) * norm;
m_a[1] = 2 * (V * K * K - 1) * norm;
m_a[2] = (1 - std11::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 + std11::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 - std11::sqrt(2*V) * K + V * K * K) * norm;
}
break;
case filterType_highShelf:
if (_gain >= 0) {
norm = 1 / (1 + M_SQRT2 * K + K * K);
m_a[0] = (V + std11::sqrt(2*V) * K + K * K) * norm;
m_a[1] = 2 * (K * K - V) * norm;
m_a[2] = (V - std11::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 + std11::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 - std11::sqrt(2*V) * K + K * K) * norm;
}
break;
}
}
void audio::drain::BiQuadFloat::reset() {
m_x[0] = 0;
m_y[1] = 0;
m_x[0] = 0;
m_y[1] = 0;
}
void audio::drain::BiQuadFloat::setBiquadCoef(float _a0, float _a1, float _a2, float _b0, float _b1) {
m_a[0] = _a0;
m_a[1] = _a1;
m_a[2] = _a2;
m_b[0] = _b0;
m_b[1] = _b1;
reset();
}
void audio::drain::BiQuadFloat::getBiquadCoef(float& _a0, float& _a1, float& _a2, float& _b0, float& _b1) {
_a0 = m_a[0];
_a1 = m_a[1];
_a2 = m_a[2];
_b0 = m_b[0];
_b1 = m_b[1];
}
std::vector<float> audio::drain::BiQuadFloat::getCoef() {
std::vector<float> out;
out.push_back(m_a[0]);
out.push_back(m_a[1]);
out.push_back(m_a[2]);
out.push_back(m_b[0]);
out.push_back(m_b[1]);
return out;
}
std::vector<std::pair<float,float> > audio::drain::BiQuadFloat::calculateTheory(double _sampleRate) {
std::vector<std::pair<float,float> > out;
double norm;
bool buildLinear = true;
size_t len = 512;
for (size_t iii=0; iii < len; iii++) {
double w;
if (buildLinear == true) {
// 0 to pi, linear scale
w = iii / (len - 1.0) * M_PI;
} else {
// 0.001 to 1, times pi, log scale
w = std11::exp(std11::log(1.0 / 0.001) * iii / (len - 1.0)) * 0.001 * M_PI;
}
double freq = iii / (len - 1.0) * _sampleRate / 2.0;
double phi = std11::pow(std11::sin(w/2.0), 2.0);
double y = std11::log( std11::pow(m_a[0]+m_a[1]+m_a[2], 2.0)
- 4.0*(m_a[0]*m_a[1] + 4.0*m_a[0]*m_a[2] + m_a[1]*m_a[2])*phi
+ 16.0*m_a[0]*m_a[2]*phi*phi)
- std11::log( std11::pow(1.0+m_b[0]+m_b[1], 2.0)
- 4.0*(m_b[0] + 4.0*m_b[1] + m_b[0]*m_b[1])*phi
+ 16.0*m_b[1]*phi*phi);
y = y * 10.0 / M_LN10;
if (y <= -200) {
y = -200.0;
}
//APPL_DEBUG("theory = " << freq << " power=" << y);
out.push_back(std::make_pair<float,float>(freq, y));
}
return out;
}

View File

@ -1,97 +0,0 @@
/** @file
* @author Edouard DUPIN
* @copyright 2011, Edouard DUPIN, all right reserved
* @license APACHE v2.0 (see license file)
*/
#ifndef __AUDIO_DRAIN_ALGO_BIQUAD_FLOAT_H__
#define __AUDIO_DRAIN_ALGO_BIQUAD_FLOAT_H__
#include <audio/drain/Algo.h>
#include <etk/memory.h>
namespace audio {
namespace drain {
enum filterType {
filterType_none, //!< no filter (pass threw...)
filterType_lowPass, //!< low pass filter
filterType_highPass, //!< High pass filter
filterType_bandPass, //!< band pass filter
filterType_notch, //!< Notch Filter
filterType_peak, //!< Peaking band EQ filter
filterType_lowShelf, //!< Low shelf filter
filterType_highShelf, //!< High shelf filter
};
class BiQuadFloat {
public:
BiQuadFloat();
protected:
float m_x[2]; //!< X history
float m_y[2]; //!< Y histiry
float m_a[3]; //!< A bi-Quad coef
float m_b[2]; //!< B bi-Quad coef
public:
/**
* @brief Set the bi-quad value and type
* @param[in] _type Type of biquad.
* @param[in] _frequencyCut Cut Frequency. [0..sampleRate/2]
* @param[in] _qualityFactor Q factor of quality (good value of 0.707 ==> permit to not ower gain) limit [0.01 .. 10]
* @param[in] _gain Gain to apply (for notch, peak, lowShelf and highShelf) limit : -30, +30
* @param[in] _sampleRate Sample rate of the signal
*/
void setBiquad(enum audio::drain::filterType _type, double _frequencyCut, double _qualityFactor, double _gain, float _sampleRate);
/**
* @brief Set direct Coefficients
*/
void setBiquadCoef(float _a0, float _a1, float _a2, float _b0, float _b1);
/**
* @brief Get direct Coefficients
*/
void getBiquadCoef(float& _a0, float& _a1, float& _a2, float& _b0, float& _b1);
/**
* @brief Get direct Coefficients
*/
std::vector<float> getCoef();
/**
* @brief Reset bequad filter (only history not value).
*/
void reset();
protected:
/**
* @brief process single sample in float.
* @param[in] _sample Sample to process
* @return updataed value
*/
float processFloat(float _sample);
public:
/**
* @brief Porcess function.
* param[in] _input Pointer on the input data.
* param[in,out] _output Poirter on the output data (can be the same as input (inplace availlable).
* param[in] _nbChunk Number of qample to process.
* param[in] _inputOffset Offset to add when read input data.
* param[in] _outputOffset Offset to add when write output data.
*/
void processFloat(float* _input,
float* _output,
size_t _nbChunk,
int32_t _inputOffset,
int32_t _outputOffset);
//! @previous
void processInt16(int16_t* _input,
int16_t* _output,
size_t _nbChunk,
int32_t _inputOffset,
int32_t _outputOffset);
/**
* @brief calculate respond of the filter:
* @param[in] _sampleRate input qample rate
* @retrun list of frequency/power in dB
*/
std::vector<std::pair<float,float> > calculateTheory(double _sampleRate);
};
}
}
#endif

View File

@ -19,6 +19,7 @@ void audio::drain::Equalizer::init() {
audio::drain::Algo::init();
audio::drain::Algo::m_type = "Equalizer";
m_supportedFormat.push_back(audio::format_int16);
m_supportedFormat.push_back(audio::format_int16);
configureBiQuad();
}
@ -34,9 +35,7 @@ audio::drain::Equalizer::~Equalizer() {
void audio::drain::Equalizer::configurationChange() {
audio::drain::Algo::configurationChange();
if (m_biquads.size() != getOutputFormat().getMap().size()) {
configureBiQuad();
}
configureBiQuad();
}
bool audio::drain::Equalizer::process(audio::Time& _time,
@ -49,27 +48,7 @@ bool audio::drain::Equalizer::process(audio::Time& _time,
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<m_biquads[jjj].size(); ++iii) {
m_biquads[jjj][iii].processFloat(data, data, _inputNbChunk, getInputFormat().getMap().size(), 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<m_biquads[jjj].size(); ++iii) {
m_biquads[jjj][iii].processInt16(data, data, _inputNbChunk, getInputFormat().getMap().size(), getOutputFormat().getMap().size() );
}
}
}
m_algo.process(_output, _input, _inputNbChunk);
return true;
}
@ -79,11 +58,7 @@ bool audio::drain::Equalizer::setParameter(const std::string& _parameter, const
m_config = ejson::Object::create(_value);
configureBiQuad();
} else if (_parameter == "reset") {
for (int32_t iii=0; iii<m_biquads.size(); ++iii) {
for (int32_t jjj=0; jjj<m_biquads[iii].size(); ++jjj) {
m_biquads[iii][jjj].reset();
}
}
// TODO : m_algo.reset();
return true;
}
return false;
@ -97,8 +72,7 @@ std::string audio::drain::Equalizer::getParameterProperty(const std::string& _pa
return "error";
}
static audio::drain::BiQuadFloat getBiquad(const std11::shared_ptr<const ejson::Object>& _object, float _frequency) {
audio::drain::BiQuadFloat out;
void audio::drain::Equalizer::addBiquad(int32_t _idBiquad, const std11::shared_ptr<const ejson::Object>& _object) {
// get type:
std::string typeString = _object->getStringValue("type", "none");
if (typeString == "direct-value") {
@ -107,23 +81,31 @@ static audio::drain::BiQuadFloat getBiquad(const std11::shared_ptr<const ejson::
double a2 = _object->getNumberValue("a2", 0.0);
double b0 = _object->getNumberValue("b0", 0.0);
double b1 = _object->getNumberValue("b1", 0.0);
out.setBiquadCoef(a0, a1, a2, b0, b1);
if (_idBiquad < 0) {
m_algo.addBiquad(a0, a1, a2, b0, b1);
} else {
m_algo.addBiquad(_idBiquad, a0, a1, a2, b0, b1);
}
} else {
enum audio::drain::filterType type;
enum audio::algo::drain::biQuadType type;
if (etk::from_string(type, typeString) == false) {
DRAIN_ERROR("Can not parse equalizer type:'" << typeString << "'");
}
double gain = _object->getNumberValue("gain", 0.0);
double frequency = _object->getNumberValue("cut-frequency", 0.0);
double quality = _object->getNumberValue("quality", 0.0);
out.setBiquad(type, frequency, quality, gain, _frequency);
if (_idBiquad < 0) {
m_algo.addBiquad(type, frequency, quality, gain);
} else {
m_algo.addBiquad(_idBiquad, type, frequency, quality, gain);
}
}
return out;
}
void audio::drain::Equalizer::configureBiQuad() {
m_biquads.clear();
m_biquads.resize(getOutputFormat().getMap().size());
m_algo.init(getOutputFormat().getFrequency(),
getOutputFormat().getMap().size(),
getOutputFormat().getFormat());
if (m_config == nullptr) {
return;
}
@ -137,19 +119,14 @@ void audio::drain::Equalizer::configureBiQuad() {
DRAIN_ERROR("Parse the configuration error : not a correct parameter:" << kkk);
continue;
}
// declare biquad:
audio::drain::BiQuadFloat biquad = getBiquad(tmpObject, getOutputFormat().getFrequency());
// add this bequad for every Channel:
for (size_t iii=0; iii<m_biquads.size(); ++iii) {
m_biquads[iii].push_back(biquad);
}
// Add biquad...
addBiquad(-1, tmpObject);
}
return;
}
for (size_t iii=0; iii<getOutputFormat().getMap().size(); ++iii) {
std::string channelName = etk::to_string(getOutputFormat().getMap()[iii]);
const std11::shared_ptr<const ejson::Array> channelConfig = m_config->getArray(channelName);
if (channelConfig == nullptr) {
// no config ... not a problem ...
continue;
@ -161,26 +138,13 @@ void audio::drain::Equalizer::configureBiQuad() {
DRAIN_ERROR("Parse the configuration error : not a correct parameter:" << kkk);
continue;
}
// declare biquad:
audio::drain::BiQuadFloat biquad = getBiquad(tmpObject, getOutputFormat().getFrequency());
// add this bequad for specific channel:
m_biquads[iii].push_back(biquad);
// add biquad:
addBiquad(kkk, tmpObject);
}
}
return;
}
std::vector<std::pair<float,float> > audio::drain::Equalizer::calculateTheory() {
std::vector<std::pair<float,float> > out;
for (size_t iii=0; iii<m_biquads[0].size(); ++iii) {
if (iii == 0) {
out = m_biquads[0][iii].calculateTheory(getOutputFormat().getFrequency());
} else {
std::vector<std::pair<float,float> > tmp = m_biquads[0][iii].calculateTheory(getOutputFormat().getFrequency());
for (size_t jjj=0; jjj< out.size(); ++jjj) {
out[jjj].second += tmp[jjj].second;
}
}
}
return out;
return m_algo.calculateTheory();
}

View File

@ -10,7 +10,7 @@
#include <audio/drain/Algo.h>
#include <etk/memory.h>
#include <ejson/Object.h>
#include <audio/drain/BiQuadFloat.h>
#include <audio/algo/drain/Equalizer.h>
namespace audio {
namespace drain {
@ -41,14 +41,9 @@ namespace audio {
virtual bool setParameter(const std::string& _parameter, const std::string& _value);
virtual std::string getParameter(const std::string& _parameter) const;
virtual std::string getParameterProperty(const std::string& _parameter) const;
protected:
/**
* @brief repesent all the biquad to process:
* The first vector represent the number of channel to process
* The second vector represent the number of biquad to process
*/
std::vector<std::vector<BiQuadFloat> > m_biquads;
void addBiquad(int32_t _idBiquad, const std11::shared_ptr<const ejson::Object>& _object);
audio::algo::drain::Equalizer m_algo;
/**
* @brief Configure biquad with the user spec.
*/

View File

@ -14,7 +14,6 @@ def create(target):
'audio/drain/debug.cpp',
'audio/drain/airtalgo.cpp',
'audio/drain/Algo.cpp',
'audio/drain/BiQuadFloat.cpp',
'audio/drain/ChannelReorder.cpp',
'audio/drain/CircularBuffer.cpp',
'audio/drain/EndPointCallback.cpp',
@ -32,7 +31,7 @@ def create(target):
# TODO: myModule.add_optional_module_depend('speexdsp', "HAVE_SPEEX_DSP_RESAMPLE")
myModule.compile_flags_CC("-DHAVE_SPEEX_DSP_RESAMPLE")
myModule.add_module_depend(['etk', 'audio', 'ejson', 'speexdsp'])
myModule.add_module_depend(['etk', 'audio', 'ejson', 'speexdsp', 'audio_algo_drain'])
myModule.add_export_path(tools.get_current_path(__file__))
# add the currrent module at the