arduino-audio-tools
Loading...
Searching...
No Matches
GoerzelStream.h
1#pragma once
2
3#include <math.h>
4
5#include "AudioStreams.h"
6
7#ifndef M_PI
8#define M_PI (3.14159265358979323846f)
9#endif
10
17namespace audio_tools {
18
30struct GoertzelConfig : public AudioInfo {
32 float target_frequency = 0.0f;
35 int block_size = 205;
38 float threshold = 0.5f;
40 float volume = 1.0f;
42 uint8_t channel = 0;
43
44 GoertzelConfig() = default;
46 GoertzelConfig(const AudioInfo& info) : AudioInfo(info) {
47 target_frequency = 440.0f;
48 block_size = 205;
49 threshold = 0.5f;
50 }
51};
52
69 public:
70 GoertzelDetector() = default;
71
76 bool begin(const GoertzelConfig& config) {
77 this->config = config;
78 this->sample_count = 0;
79
80 if (config.target_frequency == 0.0f) {
81 return false;
82 }
83
84 // Calculate the coefficient k and omega
85 float omega = (2.0f * M_PI * config.target_frequency) / config.sample_rate;
86 coeff = 2.0f * cos(omega);
87
88 // Reset state variables
89 reset();
90 return true;
91 }
92
98 bool processSample(float sample) {
99 // Goertzel algorithm core computation
100 float s0 = sample + coeff * s1 - s2;
101 s2 = s1;
102 s1 = s0;
103
104 sample_count++;
105
106 // Check if we've processed a complete block
107 if (sample_count >= config.block_size) {
108 // Calculate magnitude squared
109 float real = s1 - s2 * cos(2.0f * M_PI * config.target_frequency /
110 config.sample_rate);
111 float imag =
112 s2 * sin(2.0f * M_PI * config.target_frequency / config.sample_rate);
113 magnitude_squared = (real * real) + (imag * imag);
114 magnitude = sqrt(magnitude_squared);
115
116 // Reset for next block
117 reset();
118 return true;
119 }
120
121 return false;
122 }
123
128 float getMagnitude() const { return magnitude; }
129
135 float getMagnitudeSquared() const { return magnitude_squared; }
136
142 bool isDetected(float threshold) const { return magnitude > threshold; }
143
148 bool isDetected() const { return isDetected(config.threshold); }
149
153 void reset() {
154 s1 = 0.0f;
155 s2 = 0.0f;
156 sample_count = 0;
157 magnitude_squared = 0.0f;
158 }
159
163 float getTargetFrequency() const { return config.target_frequency; }
164
168 int getBlockSize() const { return config.block_size; }
169
173 const GoertzelConfig& getConfig() const { return config; }
174
175 void setReference(void* ref) { this->reference = ref; }
176
177 void* getReference() { return reference; }
178
179 protected:
180 GoertzelConfig config;
181 float coeff = 0.0f;
182 void* reference = nullptr;
183
184 // State variables
185 float s1 = 0.0f;
186 float s2 = 0.0f;
187 int sample_count = 0;
188
189 // Results
190 float magnitude = 0.0f;
191 float magnitude_squared = 0.0f;
192};
193
232 public:
233 // Default Constructor with no output or input
234 GoertzelStream() = default;
235 GoertzelStream(Print& out) { setOutput(out); }
236 GoertzelStream(AudioOutput& out) { setOutput(out); }
237 GoertzelStream(Stream& io) { setStream(io); };
239
250 void setAudioInfo(AudioInfo info) override {
253 // Initialize detectors for each frequency
254 begin();
255 }
256
267 GoertzelConfig result;
268 return result;
269 }
270
281 bool begin(GoertzelConfig config) {
282 config.target_frequency = 0;
283 if (config.channel >= config.channels) {
284 LOGE("GoertzelStream: channel %d out of range (max %d)", config.channel,
285 config.channels);
286 return false;
287 }
288 this->default_config = config;
289
290 // Set up audio info from config
291 setAudioInfo(config);
292 return begin();
293 }
294
303 bool begin() {
304 int i = 0;
305 detectors.clear();
306 for (float freq : frequencies) {
308 cfg.target_frequency = freq;
309 GoertzelDetector detector;
310 if (i < references.size()) {
311 detector.setReference(references[i]);
312 }
313 detector.begin(cfg);
314 detectors.push_back(detector);
315 i++;
316 }
317 sample_no = 0;
318 return true;
319 }
320
324 void end() {
325 detectors.clear();
326 AudioStream::end();
327 }
328
330 void setStream(Stream& in) {
331 p_stream = &in;
332 p_print = &in;
333 }
334
337 setStream((Stream&)io);
339 }
340
342 void setOutput(Print& out) { p_print = &out; }
343
346 setOutput((Print&)out);
348 }
349
361 void setFrequencyDetectionCallback(void (*callback)(float frequency,
362 float magnitude,
363 void* ref)) {
365 }
366
379 size_t write(const uint8_t* data, size_t len) override {
380 // Process samples for detection
381 processSamples(data, len);
382
384 if (p_print == nullptr) return len;
385
386 // Pass data through unchanged
387 return p_print->write(data, len);
388 }
389
402 size_t readBytes(uint8_t* data, size_t len) override {
403 if (p_stream == nullptr) return 0;
404
405 size_t result = p_stream->readBytes(data, len);
406
407 // Process samples for detection
408 processSamples(data, result);
409
410 return result;
411 }
412
421 const GoertzelConfig& getConfig() const { return default_config; }
422
432 void setReference(void* ref) { this->ref = ref; }
433
443 GoertzelDetector& getDetector(int no) { return detectors[no]; }
444
450 void addFrequency(float freq) { frequencies.push_back(freq); }
451
464 void addFrequency(float freq, void* ref) {
465 frequencies.push_back(freq);
466 references.push_back(ref);
467 }
468
469 protected:
470 // Core detection components
476 // Stream I/O components
477 Stream* p_stream = nullptr;
478 Print* p_print = nullptr;
479
480 // Callback system
481 void (*frequency_detection_callback)(float frequency, float magnitude,
482 void* ref) =
483 nullptr;
484 void* ref = this;
485
486 size_t sample_no = 0;
487
492 float magnitude = detector.getMagnitude();
493 if (magnitude > 0.0f)
494 LOGD("frequency: %f / magnitude: %f / threshold: %f", detector.getTargetFrequency(), magnitude, default_config.threshold);
495
496 if (magnitude > default_config.threshold) {
497 float frequency = detector.getTargetFrequency();
498 void* reference = detector.getReference();
499 if (reference == nullptr) reference = ref;
500 if (frequency_detection_callback != nullptr) {
501 frequency_detection_callback(frequency, magnitude, reference);
502 }
503 }
504 }
505
519 template <typename T>
520 void processSamplesOfType(const uint8_t* data, size_t data_len,
521 int channels) {
522 const T* samples = reinterpret_cast<const T*>(data);
523 size_t num_samples = data_len / sizeof(T);
524
525 for (size_t i = 0; i < num_samples; i++) {
526 // select only samples for the configured channel
527 if (sample_no % channels == default_config.channel) {
528 float normalized = clip(NumberConverter::toFloatT<T>(samples[i]) *
530 LOGD("sample: %f", normalized);
531 // process all frequencies
532 for (auto& detector : detectors) {
533 if (detector.processSample(normalized)) {
534 checkDetection(detector);
535 }
536 }
537 }
538 sample_no++;
539 }
540 }
541
551 float clip(float value) {
552 // Clip the value to the range [-1.0, 1.0]
553 if (value > 1.0f) return 1.0f;
554 if (value < -1.0f) return -1.0f;
555 return value;
556 }
557
575 void processSamples(const uint8_t* data, size_t data_len) {
576 // return if there is nothing to detect
577 if (detectors.empty()) return;
578 int channels = default_config.channels;
579
581 case 8:
582 processSamplesOfType<uint8_t>(data, data_len, channels);
583 break;
584 case 16:
585 processSamplesOfType<int16_t>(data, data_len, channels);
586 break;
587 case 24:
588 processSamplesOfType<int24_t>(data, data_len, channels);
589 break;
590 case 32:
591 processSamplesOfType<int32_t>(data, data_len, channels);
592 break;
593 default:
594 LOGE("Unsupported bits_per_sample: %d", default_config.bits_per_sample);
595 break;
596 }
597 }
598};
599
600} // namespace audio_tools
virtual void addNotifyAudioChange(AudioInfoSupport &bi)
Adds target to be notified about audio changes.
Definition AudioTypes.h:153
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:122
virtual void setAudioInfo(AudioInfo newInfo) override
Defines the input AudioInfo.
Definition BaseStream.h:130
Single-frequency Goertzel algorithm implementation for tone detection. The Goertzel algorithm is an e...
Definition GoerzelStream.h:68
int getBlockSize() const
Get the block size.
Definition GoerzelStream.h:168
bool isDetected() const
Check if the detected magnitude is above the configured threshold.
Definition GoerzelStream.h:148
bool begin(const GoertzelConfig &config)
Initialize the Goertzel detector with configuration.
Definition GoerzelStream.h:76
float getMagnitudeSquared() const
Get the squared magnitude (more efficient if you don't need the actual magnitude)
Definition GoerzelStream.h:135
bool processSample(float sample)
Process a single sample.
Definition GoerzelStream.h:98
bool isDetected(float threshold) const
Check if the detected magnitude is above a threshold.
Definition GoerzelStream.h:142
const GoertzelConfig & getConfig() const
Get the current configuration.
Definition GoerzelStream.h:173
float getMagnitude() const
Get the magnitude of the detected frequency.
Definition GoerzelStream.h:128
float getTargetFrequency() const
Get the target frequency.
Definition GoerzelStream.h:163
void reset()
Reset the detector state.
Definition GoerzelStream.h:153
AudioStream-based multi-frequency Goertzel detector for real-time audio analysis.
Definition GoerzelStream.h:231
void setStream(AudioStream &io)
Defines/Changes the input & output.
Definition GoerzelStream.h:336
void * ref
User-defined reference for callback context.
Definition GoerzelStream.h:484
void processSamplesOfType(const uint8_t *data, size_t data_len, int channels)
Template helper to process samples of a specific type.
Definition GoerzelStream.h:520
GoertzelDetector & getDetector(int no)
Get detector for specific channel.
Definition GoerzelStream.h:443
void addFrequency(float freq, void *ref)
Add a frequency to the detection list with a custom reference pointer.
Definition GoerzelStream.h:464
void setOutput(AudioOutput &out)
Defines/Changes the output target.
Definition GoerzelStream.h:345
size_t readBytes(uint8_t *data, size_t len) override
Read data from input stream and process it.
Definition GoerzelStream.h:402
void processSamples(const uint8_t *data, size_t data_len)
Generic sample processing method for both write and readBytes.
Definition GoerzelStream.h:575
bool begin()
Initialize detectors for all frequencies.
Definition GoerzelStream.h:303
float clip(float value)
Clip audio values to prevent overflow.
Definition GoerzelStream.h:551
size_t write(const uint8_t *data, size_t len) override
Process audio data and pass it through.
Definition GoerzelStream.h:379
Vector< void * > references
List of frequencies to detect.
Definition GoerzelStream.h:474
void(* frequency_detection_callback)(float frequency, float magnitude, void *ref)
User callback for detection events.
Definition GoerzelStream.h:481
Stream * p_stream
Input stream for reading audio data.
Definition GoerzelStream.h:477
GoertzelConfig default_config
Current algorithm configuration.
Definition GoerzelStream.h:475
Vector< float > frequencies
List of frequencies to detect.
Definition GoerzelStream.h:473
const GoertzelConfig & getConfig() const
Get the current configuration.
Definition GoerzelStream.h:421
void setReference(void *ref)
Set reference pointer for callback context.
Definition GoerzelStream.h:432
void addFrequency(float freq)
Add a frequency to the detection list.
Definition GoerzelStream.h:450
void end()
Stop the Goertzel detectors and clear resources.
Definition GoerzelStream.h:324
bool begin(GoertzelConfig config)
Initialize with GoertzelConfig.
Definition GoerzelStream.h:281
Vector< GoertzelDetector > detectors
One detector per frequency in frequencies.
Definition GoerzelStream.h:472
size_t sample_no
Sample counter for channel selection.
Definition GoerzelStream.h:486
Print * p_print
Output stream for writing audio data.
Definition GoerzelStream.h:478
void setAudioInfo(AudioInfo info) override
Set audio format and initialize detector array.
Definition GoerzelStream.h:250
void setOutput(Print &out)
Defines/Changes the output target.
Definition GoerzelStream.h:342
void setFrequencyDetectionCallback(void(*callback)(float frequency, float magnitude, void *ref))
Set detection callback function for channel-aware frequency detection.
Definition GoerzelStream.h:361
void checkDetection(GoertzelDetector &detector)
Check if a detector has detected its frequency and invoke callback.
Definition GoerzelStream.h:491
GoertzelConfig defaultConfig()
Returns a default GoertzelConfig instance with standard parameters.
Definition GoerzelStream.h:266
void setStream(Stream &in)
Defines/Changes the input & output.
Definition GoerzelStream.h:330
Definition NoArduino.h:62
Definition NoArduino.h:142
Vector implementation which provides the most important methods as defined by std::vector....
Definition Vector.h:21
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:55
void copyFrom(AudioInfo info)
Same as set.
Definition AudioTypes.h:105
sample_rate_t sample_rate
Sample Rate: e.g 44100.
Definition AudioTypes.h:57
uint16_t channels
Number of channels: 2=stereo, 1=mono.
Definition AudioTypes.h:59
uint8_t bits_per_sample
Number of bits per sample (int16_t = 16 bits)
Definition AudioTypes.h:61
Configuration for Goertzel algorithm detectors.
Definition GoerzelStream.h:30
float volume
Volume factor for normalization - scales input samples before processing.
Definition GoerzelStream.h:40
float threshold
Definition GoerzelStream.h:38
int block_size
Definition GoerzelStream.h:35
uint8_t channel
channel used for detection when used in a stream
Definition GoerzelStream.h:42
GoertzelConfig(const AudioInfo &info)
Copy constructor from AudioInfo.
Definition GoerzelStream.h:46
float target_frequency
Target frequency to detect in Hz (same for all channels)
Definition GoerzelStream.h:32