1+ #pragma once
2+
3+ #include " AudioTools/CoreAudio/AudioStreams.h"
4+ #include " AudioTools/CoreAudio/Buffers.h"
5+
6+ namespace audio_tools {
7+ /* *
8+ * @class EchoCancellation
9+ * @brief Echo cancellation with adaptive LMS filtering for microcontrollers.
10+ *
11+ * This class implements echo cancellation using an adaptive FIR filter (LMS
12+ * algorithm). It estimates the echo path and subtracts the estimated echo from
13+ * the microphone input.
14+ */
15+ template <typename T = int16_t >
16+ class EchoCancellation : public AudioStream {
17+ public:
18+ /* *
19+ * @brief Constructor
20+ * @param in Reference to the input stream (microphone or audio input)
21+ * @param lag_samples Number of samples to delay the echo subtraction
22+ * (default: 0)
23+ * @param buffer_size Size of the internal ring buffer (default: 512)
24+ */
25+ EchoCancellation (Stream& in, size_t lag_samples = 0 , size_t buffer_size = 512 ,
26+ size_t filter_len = 32 , float mu = 0 .001f )
27+ : lag(lag_samples),
28+ buffer_size (buffer_size),
29+ filter_len(filter_len),
30+ adaptation_rate(mu) {
31+ p_io = ∈
32+ filter.resize (filter_len, 0 .0f );
33+ reset ();
34+ }
35+
36+ /* *
37+ * @brief Store the output signal (sent to speaker)
38+ * @param buf Pointer to PCM data sent to the speaker (T*)
39+ * @param len Number of bytes in buf
40+ * @return Number of bytes processed
41+ */
42+ size_t write (const uint8_t * buf, size_t len) override {
43+ // Store output signal in queue for echo estimation
44+ return ring_buffer.writeArray ((T*)buf, len / sizeof (T)) *
45+ sizeof (T);
46+ }
47+
48+ /* *
49+ * @brief Read input and remove echo (subtract output signal with lag)
50+ * @param buf Pointer to buffer to store processed input (T*)
51+ * @param len Number of bytes to read
52+ * @return Number of bytes read from input
53+ */
54+ size_t readBytes (uint8_t * buf, size_t len) override {
55+ size_t read = p_io->readBytes (buf, len);
56+ size_t actual_samples = read / sizeof (T);
57+ T* data = (T*)buf;
58+ Vector<T> ref_vec (filter_len, 0 );
59+ ring_buffer.peekArray (ref_vec.data (), filter_len);
60+ for (size_t i = 0 ; i < actual_samples; ++i) {
61+ // Build the reference vector for the adaptive filter
62+ float echo_est = 0 .0f ;
63+ for (size_t k = 0 ; k < filter_len; ++k) {
64+ echo_est += filter[k] * ref_vec[k];
65+ }
66+ float mic = (float )data[i];
67+ float error = mic - echo_est;
68+ data[i] = (T)error;
69+ // LMS update
70+ for (size_t k = 0 ; k < filter_len; ++k) {
71+ filter[k] += adaptation_rate * error * ref_vec[k];
72+ }
73+ T dummy;
74+ ring_buffer.read (dummy); // Advance the queue
75+ // Shift ref_vec left and append dummy
76+ for (size_t k = 0 ; k < filter_len - 1 ; ++k) {
77+ ref_vec[k] = ref_vec[k + 1 ];
78+ }
79+ ref_vec[filter_len - 1 ] = dummy;
80+ }
81+ return read;
82+ }
83+
84+ /* *
85+ * @brief Set the lag (delay) in samples for echo cancellation.
86+ * @param lag_samples Number of samples to delay the echo subtraction
87+ */
88+ void setLag (size_t lag_samples) { lag = lag_samples; }
89+
90+ /* *
91+ * @brief Set the adaptation rate (mu) for the LMS algorithm.
92+ * @param mu Adaptation rate
93+ */
94+ void setMu (float mu) { adaptation_rate = mu; }
95+
96+ /* *
97+ * @brief Set the filter length for the adaptive filter.
98+ * @param len Length of the adaptive filter
99+ */
100+ void setFilterLen (size_t len) {
101+ filter_len = len;
102+ filter.resize (filter_len, 0 .0f );
103+ }
104+
105+ /* *
106+ * @brief Reset the internal buffer and lag state.
107+ */
108+ void reset () {
109+ ring_buffer.resize (buffer_size + lag);
110+ for (size_t j = 0 ; j < lag; j++) {
111+ ring_buffer.write (0 );
112+ }
113+ filter.assign (filter_len, 0 .0f );
114+ }
115+
116+ protected:
117+ Stream* p_io = nullptr ;
118+ RingBuffer<T> ring_buffer{0 };
119+ size_t buffer_size;
120+ size_t lag; // lag in samples
121+ // Adaptive filter
122+ size_t filter_len;
123+ float adaptation_rate = 0 .01f ;
124+ Vector<float > filter;
125+ };
126+
127+ } // namespace audio_tools
0 commit comments