mirror of
https://github.com/mudita/MuditaOS.git
synced 2026-01-18 02:48:05 -05:00
dd3eeaf1e2
Fixes to major part of issues found by PVS Studio static analysis mainly micro-optimisations
122 lines
4.8 KiB
C++
122 lines
4.8 KiB
C++
// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
|
|
// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
|
|
|
|
#include "Equalizer.hpp"
|
|
|
|
#include <cstdint>
|
|
#include <cmath>
|
|
#include <stdexcept>
|
|
|
|
namespace audio::equalizer
|
|
{
|
|
QFilterCoefficients qfilter_CalculateCoeffs(
|
|
FilterType filter, float frequency, uint32_t samplerate, float Q, float gain)
|
|
{
|
|
constexpr auto qMinValue = .1f;
|
|
constexpr auto qMaxValue = 10.f;
|
|
constexpr auto frequencyMinValue = 0.f;
|
|
if (frequency < frequencyMinValue && filter != FilterType::None) {
|
|
throw std::invalid_argument("Negative frequency provided");
|
|
}
|
|
if ((Q < qMinValue || Q > qMaxValue) && filter != FilterType::None) {
|
|
throw std::invalid_argument("Q out of range");
|
|
}
|
|
if (samplerate == 0 && filter != FilterType::None) {
|
|
throw std::invalid_argument("Sample rate has to be greater than zero");
|
|
}
|
|
QFilterCoefficients filter_coeff;
|
|
float a0 = 0;
|
|
float omega = 2 * M_PI * frequency / samplerate;
|
|
float sn = sin(omega);
|
|
float cs = cos(omega);
|
|
float alpha = sn / (2 * Q);
|
|
float gain_abs = pow(10, gain / 40);
|
|
|
|
switch (filter) {
|
|
case FilterType::BandPass:
|
|
filter_coeff.b0 = alpha;
|
|
filter_coeff.b1 = 0;
|
|
filter_coeff.b2 = -alpha;
|
|
filter_coeff.a1 = -2 * cs;
|
|
filter_coeff.a2 = 1 - alpha;
|
|
a0 = 1 + alpha;
|
|
break;
|
|
|
|
case FilterType::HighPass:
|
|
filter_coeff.b0 = (1 + cs) / 2;
|
|
filter_coeff.b1 = -(1 + cs);
|
|
filter_coeff.b2 = (1 + cs) / 2;
|
|
filter_coeff.a1 = -2 * cs;
|
|
filter_coeff.a2 = 1 - alpha;
|
|
a0 = 1 + alpha;
|
|
break;
|
|
|
|
case FilterType::LowPass:
|
|
filter_coeff.b0 = (1 - cs) / 2;
|
|
filter_coeff.b1 = 1 - cs;
|
|
filter_coeff.b2 = (1 - cs) / 2;
|
|
filter_coeff.a1 = -2 * cs;
|
|
filter_coeff.a2 = 1 - alpha;
|
|
a0 = 1 + alpha;
|
|
break;
|
|
case FilterType::Flat:
|
|
filter_coeff.b0 = 0.0;
|
|
filter_coeff.b1 = 0.0;
|
|
filter_coeff.b2 = 1;
|
|
filter_coeff.a1 = 0.0;
|
|
filter_coeff.a2 = 0.0;
|
|
a0 = 1;
|
|
break;
|
|
case FilterType::Notch:
|
|
filter_coeff.b0 = 1;
|
|
filter_coeff.b1 = -2 * cs;
|
|
filter_coeff.b2 = 1;
|
|
filter_coeff.a1 = -2 * cs;
|
|
filter_coeff.a2 = 1 - alpha;
|
|
a0 = 1 + alpha;
|
|
break;
|
|
case FilterType::HighShelf:
|
|
filter_coeff.b0 = gain_abs * ((gain_abs + 1) + (gain_abs - 1) * cs + 2 * sqrt(gain_abs) * alpha);
|
|
filter_coeff.b1 = -2 * gain_abs * ((gain_abs - 1.0) + (gain_abs + 1) * cs);
|
|
filter_coeff.b2 = gain_abs * ((gain_abs + 1) + (gain_abs - 1) * cs - 2 * sqrt(gain_abs) * alpha);
|
|
a0 = (gain_abs + 1) - (gain_abs - 1) * cs + 2 * sqrt(gain_abs) * alpha;
|
|
filter_coeff.a1 = 2 * ((gain_abs - 1) - (gain_abs + 1) * cs);
|
|
filter_coeff.a2 = (gain_abs + 1) - (gain_abs - 1) * cs - 2 * sqrt(gain_abs) * alpha;
|
|
break;
|
|
case FilterType::LowShelf:
|
|
filter_coeff.b0 = gain_abs * ((gain_abs + 1) - (gain_abs - 1) * cs + 2 * sqrt(gain_abs) * alpha);
|
|
filter_coeff.b1 = 2 * gain_abs * ((gain_abs - 1.0) - (gain_abs + 1) * cs);
|
|
filter_coeff.b2 = gain_abs * ((gain_abs + 1) - (gain_abs - 1) * cs - 2 * sqrt(gain_abs) * alpha);
|
|
a0 = (gain_abs + 1) + (gain_abs - 1) * cs + 2 * sqrt(gain_abs) * alpha;
|
|
filter_coeff.a1 = -2 * ((gain_abs - 1) + (gain_abs + 1) * cs);
|
|
filter_coeff.a2 = (gain_abs + 1) + (gain_abs - 1) * cs - 2 * sqrt(gain_abs) * alpha;
|
|
break;
|
|
case FilterType::Parametric:
|
|
filter_coeff.b0 = 1.0 + alpha * gain_abs;
|
|
filter_coeff.b1 = -2.0 * cs;
|
|
filter_coeff.b2 = 1.0 - alpha * gain_abs;
|
|
a0 = 1.0 + alpha / gain_abs;
|
|
filter_coeff.a1 = -2.0 * cs;
|
|
filter_coeff.a2 = 1.0 - alpha / gain_abs;
|
|
break;
|
|
case FilterType::None:
|
|
filter_coeff.b0 = 1;
|
|
filter_coeff.b1 = 0;
|
|
filter_coeff.b2 = 0;
|
|
a0 = 1;
|
|
filter_coeff.a1 = 0;
|
|
filter_coeff.a2 = 0;
|
|
break;
|
|
}
|
|
|
|
// prescale flter constants
|
|
filter_coeff.b0 /= a0;
|
|
filter_coeff.b1 /= a0;
|
|
filter_coeff.b2 /= a0;
|
|
filter_coeff.a1 /= a0;
|
|
filter_coeff.a2 /= a0;
|
|
|
|
return filter_coeff;
|
|
}
|
|
} // namespace audio::equalizer
|