ESPHome 2026.11.0b6
Loading...
Searching...
No Matches
dc_offset_filter.h
Go to the documentation of this file.
1#pragma once
2
4
5#include <algorithm>
6#include <array>
7#include <cmath>
8#include <cstddef>
9#include <cstdint>
10#include <limits>
11#include <numbers>
12
13namespace esphome::i2s_audio {
14
17 public:
18 void reset(uint32_t sample_rate) {
19 // A 20 Hz pole gives the same settling time at every sample rate. Normalize the
20 // feedforward coefficient to keep unity gain at Nyquist: b = (1 + a) / 2.
21 // Single precision keeps double-precision exp out of the build; expm1 gives 1 - a accurately with a near 1.
22 const float half_one_minus_pole =
23 -0.5f * std::expm1(-2.0f * std::numbers::pi_v<float> * 20.0f / static_cast<float>(sample_rate));
24 // Round to nearest in integers: half of (twice the value, truncated, plus one)
25 const int64_t twice = static_cast<int64_t>(half_one_minus_pole * (2 * COEFFICIENT_SCALE));
26 this->feedforward_ = static_cast<int32_t>(COEFFICIENT_SCALE - ((twice + 1) >> 1));
27 this->feedback_ = static_cast<int32_t>(2 * static_cast<int64_t>(this->feedforward_) - COEFFICIENT_SCALE);
28 this->states_ = {};
29 }
30
31 // The I2S driver supplies complete mono or stereo frames with the sample rate used by reset().
32 void process(uint8_t *data, size_t size, const audio::AudioStreamInfo &stream_info) {
33 const size_t bytes_per_sample = stream_info.samples_to_bytes(1);
34 const size_t channels = stream_info.get_channels();
35 // Only mono and stereo streams have channel histories; leave other formats unchanged.
36 if (channels == 0 || channels > this->states_.size()) {
37 return;
38 }
39 const size_t frames = stream_info.bytes_to_frames(size);
40 for (size_t frame = 0; frame < frames; ++frame) {
41 for (size_t channel = 0; channel < channels; ++channel) {
42 auto &state = this->states_[channel];
43 const int32_t input = audio::unpack_audio_sample_to_q31(data, bytes_per_sample);
44 // Q30 coefficients leave room for full-scale steps in the 64-bit products.
45 // Keep the unclipped history so clipping a transient does not change the filter's decay.
46 const int64_t difference = static_cast<int64_t>(input) - state.input;
47 const int64_t output = ((this->feedforward_ * difference) >> 30) + ((this->feedback_ * state.output) >> 30);
48 state.input = input;
49 state.output = output;
50 const int32_t sample = static_cast<int32_t>(
51 std::clamp<int64_t>(output, std::numeric_limits<int32_t>::min(), std::numeric_limits<int32_t>::max()));
52 audio::pack_q31_as_audio_sample(sample, data, bytes_per_sample);
53 data += bytes_per_sample;
54 }
55 }
56 }
57
58 protected:
59 static constexpr int64_t COEFFICIENT_SCALE = int64_t{1} << 30;
60 struct ChannelState {
61 int32_t input{0};
62 int64_t output{0};
63 };
64 std::array<ChannelState, 2> states_{};
65 int32_t feedforward_{0};
66 int32_t feedback_{0};
67};
68
69} // namespace esphome::i2s_audio
size_t samples_to_bytes(uint32_t samples) const
Converts samples to bytes.
Definition audio.h:58
uint32_t bytes_to_frames(size_t bytes) const
Convert bytes to frames.
Definition audio.h:43
uint8_t get_channels() const
Definition audio.h:29
Removes DC from mono or stereo PCM, retaining separate channel histories between buffers.
std::array< ChannelState, 2 > states_
void reset(uint32_t sample_rate)
void process(uint8_t *data, size_t size, const audio::AudioStreamInfo &stream_info)
static constexpr int64_t COEFFICIENT_SCALE
bool state
Definition fan.h:2
int32_t unpack_audio_sample_to_q31(const uint8_t *data, size_t bytes_per_sample)
Unpacks a quantized audio sample into a Q31 fixed-point number.
Definition audio.h:158
void pack_q31_as_audio_sample(int32_t sample, uint8_t *data, size_t bytes_per_sample)
Packs a Q31 fixed-point number as an audio sample with the specified number of bytes per sample.
Definition audio.h:184
uint16_t size
Definition helpers.cpp:34
static void uint32_t