~aleteoryx/muditaos

ref: 34ee674f371c0588f1c47bb0ce6fdda517e2ace0 muditaos/module-audio/Audio/equalizer/Equalizer.cpp -rw-r--r-- 4.8 KiB
34ee674f — Mateusz Piesta [MOS-939] Apply target update scripts 2 years ago 
                                                                                
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

// 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