EchoSuppressor.cpp revision a8a10096a1501e901676632d78f699cdebe9f4f6 
1/*
2 * Copyrightm (C) 2010 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17#include <stdio.h>
18#include <stdint.h>
19#include <math.h>
20
21#define LOG_TAG "Echo"
22#include <utils/Log.h>
23
24#include "EchoSuppressor.h"
25
26EchoSuppressor::EchoSuppressor(int sampleRate, int sampleCount, int tailLength)
27{
28    int scale = 1;
29    while (tailLength > 200 * scale) {
30        scale <<= 1;
31    }
32    if (scale > sampleCount) {
33        scale = sampleCount;
34    }
35
36    mScale = scale;
37    mSampleCount = sampleCount;
38    mWindowSize = sampleCount / scale;
39    mTailLength = (tailLength + scale - 1) / scale;
40    mRecordLength = (sampleRate + sampleCount - 1) / sampleCount;
41    mRecordOffset = 0;
42
43    mXs = new float[mTailLength + mWindowSize];
44    memset(mXs, 0, sizeof(float) * (mTailLength + mWindowSize));
45    mXYs = new float[mTailLength];
46    memset(mXYs, 0, sizeof(float) * mTailLength);
47    mXXs = new float[mTailLength];
48    memset(mXYs, 0, sizeof(float) * mTailLength);
49    mYY = 0;
50
51    mXYRecords = new float[mRecordLength * mTailLength];
52    memset(mXYRecords, 0, sizeof(float) * mRecordLength * mTailLength);
53    mXXRecords = new float[mRecordLength * mWindowSize];
54    memset(mXXRecords, 0, sizeof(float) * mRecordLength * mWindowSize);
55    mYYRecords = new float[mRecordLength];
56    memset(mYYRecords, 0, sizeof(float) * mRecordLength);
57
58    mLastX = 0;
59    mLastY = 0;
60}
61
62EchoSuppressor::~EchoSuppressor()
63{
64    delete [] mXs;
65    delete [] mXYs;
66    delete [] mXXs;
67    delete [] mXYRecords;
68    delete [] mXXRecords;
69    delete [] mYYRecords;
70}
71
72void EchoSuppressor::run(int16_t *playbacked, int16_t *recorded)
73{
74    float *records;
75
76    // Update Xs.
77    for (int i = 0; i < mTailLength; ++i) {
78        mXs[i] = mXs[mWindowSize + i];
79    }
80    for (int i = 0, j = 0; i < mWindowSize; ++i, j += mScale) {
81        float sum = 0;
82        for (int k = 0; k < mScale; ++k) {
83            float x = playbacked[j + k] >> 8;
84            mLastX += x;
85            sum += (mLastX >= 0) ? mLastX : -mLastX;
86            mLastX = 0.005f * mLastX - x;
87        }
88        mXs[mTailLength - 1 + i] = sum;
89    }
90
91    // Update XXs and XXRecords.
92    for (int i = 0; i < mTailLength - mWindowSize; ++i) {
93        mXXs[i] = mXXs[mWindowSize + i];
94    }
95    records = &mXXRecords[mRecordOffset * mWindowSize];
96    for (int i = 0, j = mTailLength - mWindowSize; i < mWindowSize; ++i, ++j) {
97        float xx = mXs[mTailLength - 1 + i] * mXs[mTailLength - 1 + i];
98        mXXs[j] = mXXs[j - 1] + xx - records[i];
99        records[i] = xx;
100        if (mXXs[j] < 0) {
101            mXXs[j] = 0;
102        }
103    }
104
105    // Compute Ys.
106    float ys[mWindowSize];
107    for (int i = 0, j = 0; i < mWindowSize; ++i, j += mScale) {
108        float sum = 0;
109        for (int k = 0; k < mScale; ++k) {
110            float y = recorded[j + k] >> 8;
111            mLastY += y;
112            sum += (mLastY >= 0) ? mLastY : -mLastY;
113            mLastY = 0.005f * mLastY - y;
114        }
115        ys[i] = sum;
116    }
117
118    // Update YY and YYRecords.
119    float yy = 0;
120    for (int i = 0; i < mWindowSize; ++i) {
121        yy += ys[i] * ys[i];
122    }
123    mYY += yy - mYYRecords[mRecordOffset];
124    mYYRecords[mRecordOffset] = yy;
125    if (mYY < 0) {
126        mYY = 0;
127    }
128
129    // Update XYs and XYRecords.
130    records = &mXYRecords[mRecordOffset * mTailLength];
131    for (int i = 0; i < mTailLength; ++i) {
132        float xy = 0;
133        for (int j = 0;j < mWindowSize; ++j) {
134            xy += mXs[i + j] * ys[j];
135        }
136        mXYs[i] += xy - records[i];
137        records[i] = xy;
138        if (mXYs[i] < 0) {
139            mXYs[i] = 0;
140        }
141    }
142
143    // Computes correlations from XYs, XXs, and YY.
144    float weight = 1.0f / (mYY + 1);
145    float correlation = 0;
146    int latency = 0;
147    for (int i = 0; i < mTailLength; ++i) {
148        float c = mXYs[i] * mXYs[i] * weight / (mXXs[i] + 1);
149        if (c > correlation) {
150            correlation = c;
151            latency = i;
152        }
153    }
154
155    correlation = sqrtf(correlation);
156    if (correlation > 0.3f) {
157        float factor = 1.0f - correlation;
158        factor *= factor;
159        for (int i = 0; i < mSampleCount; ++i) {
160            recorded[i] *= factor;
161        }
162    }
163//    LOGI("latency %5d, correlation %.10f", latency, correlation);
164
165
166    // Increase RecordOffset.
167    ++mRecordOffset;
168    if (mRecordOffset == mRecordLength) {
169        mRecordOffset = 0;
170    }
171}
172