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