arduino-audio-tools
All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Modules Pages
Equalizer.h
1#pragma once
2#include <math.h>
3
4#include "AudioToolsConfig.h"
5#include "AudioTools/CoreAudio/AudioOutput.h"
6#include "AudioTools/CoreAudio/AudioStreams.h"
7
14namespace audio_tools {
15
25 channels = 2;
26 bits_per_sample = 16;
27 sample_rate = 44100;
28 }
29
31
32 // Frequencies
33 int freq_low = 880;
34 int freq_high = 5000;
35
36 // Gain Controls
37 float gain_low = 1.0;
38 float gain_medium = 1.0;
39 float gain_high = 1.0;
40};
41
49 public:
50 Equalizer3Bands(Print &out) { setOutput(out); }
51
52 Equalizer3Bands(Stream &in) { setStream(in); }
53
55 setOutput(out);
56 out.addNotifyAudioChange(*this);
57 }
58
60 setStream(stream);
61 stream.addNotifyAudioChange(*this);
62 }
63
65 if (state != nullptr) delete[] state;
66 }
67
69 void setStream(Stream &io) override {
70 p_print = &io;
71 p_stream = &io;
72 };
73
75 void setOutput(Print &out) override { p_print = &out; }
76
77 ConfigEqualizer3Bands &config() { return cfg; }
78
79 ConfigEqualizer3Bands &defaultConfig() { return config(); }
80
81 bool begin(ConfigEqualizer3Bands &config) {
82 p_cfg = &config;
83 if (p_cfg->channels > max_state_count) {
84 if (state != nullptr) delete[] state;
85 state = new EQSTATE[p_cfg->channels];
86 max_state_count = p_cfg->channels;
87 }
88
89 // Setup state
90 for (int j = 0; j < max_state_count; j++) {
91 memset(&state[j], 0, sizeof(EQSTATE));
92
93 // Calculate filter cutoff frequencies
94 state[j].lf =
95 2 *
96 sin((float)PI * ((float)p_cfg->freq_low / (float)p_cfg->sample_rate));
97 state[j].hf = 2 * sin((float)PI * ((float)p_cfg->freq_high /
98 (float)p_cfg->sample_rate));
99 }
100 return true;
101 }
102
103 virtual void setAudioInfo(AudioInfo info) override {
104 p_cfg->sample_rate = info.sample_rate;
105 p_cfg->channels = info.channels;
106 p_cfg->bits_per_sample = info.bits_per_sample;
107 begin(*p_cfg);
108 }
109
110 size_t write(const uint8_t *data, size_t len) override {
111 filterSamples(data, len);
112 return p_print->write(data, len);
113 }
114
115 int availableForWrite() override { return p_print->availableForWrite(); }
116
118 size_t readBytes(uint8_t *data, size_t len) override {
119 size_t result = 0;
120 if (p_stream != nullptr) {
121 result = p_stream->readBytes(data, len);
122 filterSamples(data, len);
123 }
124 return result;
125 }
126
127 int available() override {
128 return p_stream != nullptr ? p_stream->available() : 0;
129 }
130
131 protected:
132 ConfigEqualizer3Bands cfg;
133 ConfigEqualizer3Bands *p_cfg = &cfg;
134 const float vsa = (1.0 / 4294967295.0); // Very small amount (Denormal Fix)
135 Print *p_print = nullptr; // support for write
136 Stream *p_stream = nullptr; // support for write
137 // AudioOutput *p_out=nullptr; // support for write
138 // AudioStream *p_in=nullptr; // support for readBytes
139 int max_state_count = 0;
140
141 struct EQSTATE {
142 // Filter #1 (Low band)
143 float lf; // Frequency
144 float f1p0; // Poles ...
145 float f1p1;
146 float f1p2;
147 float f1p3;
148
149 // Filter #2 (High band)
150 float hf; // Frequency
151 float f2p0; // Poles ...
152 float f2p1;
153 float f2p2;
154 float f2p3;
155
156 // Sample history buffer
157 float sdm1; // Sample data minus 1
158 float sdm2; // 2
159 float sdm3; // 3
160
161 } *state = nullptr;
162
163 void filterSamples(const uint8_t *data, size_t len) {
164 switch (p_cfg->bits_per_sample) {
165 case 16: {
166 int16_t *p_dataT = (int16_t *)data;
167 size_t sample_count = len / sizeof(int16_t);
168 for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
169 for (int ch = 0; ch < p_cfg->channels; ch++) {
170 p_dataT[j + ch] = NumberConverter::fromFloat(sample(state[ch], NumberConverter::toFloat(p_dataT[j + ch], 16)), 16);
171 }
172 }
173 } break;
174 case 24: {
175 int24_t *p_dataT = (int24_t *)data;
176 size_t sample_count = len / sizeof(int24_t);
177 for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
178 for (int ch = 0; ch < p_cfg->channels; ch++) {
179 p_dataT[j + ch] = NumberConverter::fromFloat(sample(state[ch], NumberConverter::toFloat(p_dataT[j + ch], 24)), 24);
180 }
181 }
182 } break;
183 case 32: {
184 int32_t *p_dataT = (int32_t *)data;
185 size_t sample_count = len / sizeof(int32_t);
186 for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
187 for (int ch = 0; ch < p_cfg->channels; ch++) {
188 p_dataT[j + ch] = NumberConverter::fromFloat(sample(state[ch], NumberConverter::toFloat(p_dataT[j + ch], 32)), 32);
189 }
190 }
191 } break;
192
193 default:
194 LOGE("Only 16 bits supported: %d", p_cfg->bits_per_sample);
195 break;
196 }
197 }
198
199
200 // calculates a single sample using the indicated state
201 float sample(EQSTATE &es, float sample) {
202 // Locals
203 float l, m, h; // Low / Mid / High - Sample Values
204 // Filter #1 (lowpass)
205 es.f1p0 += (es.lf * (sample - es.f1p0)) + vsa;
206 es.f1p1 += (es.lf * (es.f1p0 - es.f1p1));
207 es.f1p2 += (es.lf * (es.f1p1 - es.f1p2));
208 es.f1p3 += (es.lf * (es.f1p2 - es.f1p3));
209
210 l = es.f1p3;
211
212 // Filter #2 (highpass)
213 es.f2p0 += (es.hf * (sample - es.f2p0)) + vsa;
214 es.f2p1 += (es.hf * (es.f2p0 - es.f2p1));
215 es.f2p2 += (es.hf * (es.f2p1 - es.f2p2));
216 es.f2p3 += (es.hf * (es.f2p2 - es.f2p3));
217
218 h = es.sdm3 - es.f2p3;
219 // Calculate midrange (signal - (low + high))
220 m = es.sdm3 - (h + l);
221 // Scale, Combine and store
222 l *= p_cfg->gain_low;
223 m *= p_cfg->gain_medium;
224 h *= p_cfg->gain_high;
225
226 // Shuffle history buffer
227 es.sdm3 = es.sdm2;
228 es.sdm2 = es.sdm1;
229 es.sdm1 = sample;
230
231 // Return result
232 return (l + m + h);
233 }
234};
235
236} // namespace audio_tools
virtual void addNotifyAudioChange(AudioInfoSupport &bi)
Adds target to be notified about audio changes.
Definition AudioTypes.h:151
Abstract Audio Ouptut class.
Definition AudioOutput.h:22
Base class for all Audio Streams. It support the boolean operator to test if the object is ready with...
Definition BaseStream.h:119
3 Band Equalizer inspired from https://www.musicdsp.org/en/latest/Filters/236-3-band-equaliser....
Definition Equalizer.h:48
void setOutput(Print &out) override
Defines/Changes the output target.
Definition Equalizer.h:75
size_t readBytes(uint8_t *data, size_t len) override
Provides the data from all streams mixed together.
Definition Equalizer.h:118
virtual void setAudioInfo(AudioInfo info) override
Defines the input AudioInfo.
Definition Equalizer.h:103
void setStream(Stream &io) override
Defines/Changes the input & output.
Definition Equalizer.h:69
Abstract class: Objects can be put into a pipleline.
Definition AudioStreams.h:68
static float toFloat(int32_t value, int bits)
Convert an integer integer autio type to a float (with max 1.0)
Definition AudioTypes.h:374
static int32_t fromFloat(float value, int bits)
Convert a float (with max 1.0) to an integer autio type.
Definition AudioTypes.h:379
Definition NoArduino.h:62
Definition NoArduino.h:142
Generic Implementation of sound input and output for desktop environments using portaudio.
Definition AudioCodecsBase.h:10
Basic Audio information which drives e.g. I2S.
Definition AudioTypes.h:53
sample_rate_t sample_rate
Sample Rate: e.g 44100.
Definition AudioTypes.h:55
uint16_t channels
Number of channels: 2=stereo, 1=mono.
Definition AudioTypes.h:57
uint8_t bits_per_sample
Number of bits per sample (int16_t = 16 bits)
Definition AudioTypes.h:59
Configuration for 3 Band Equalizer: Set channels,bits_per_sample,sample_rate. Set and update gain_low...
Definition Equalizer.h:23
Definition Equalizer.h:141