echocancel.cpp 14.2 KB
Newer Older
1
/*
2
 *  Copyright (C) 2004, 2005, 2006, 2009, 2008, 2009, 2010 Savoir-Faire Linux Inc.
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
 *  Author: Alexandre Savard <alexandre.savard@savoirfairelinux.com>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18
19
20
21
22
23
24
25
26
27
28
 *
 *  Additional permission under GNU GPL version 3 section 7:
 *
 *  If you modify this program, or any covered work, by linking or
 *  combining it with the OpenSSL project's OpenSSL library (or a
 *  modified version of that library), containing parts covered by the
 *  terms of the OpenSSL or SSLeay licenses, Savoir-Faire Linux Inc.
 *  grants you additional permission to convey the resulting work.
 *  Corresponding Source for a non-source form of such a combination
 *  shall include the source code for the parts of OpenSSL used as well
 *  as that of the covered work.
29
30
31
32
 */

#include "echocancel.h"

33
// #include <fstream>
34

35
36
37
38
39
EchoCancel::EchoCancel(int smplRate, int frameLength) : _samplingRate(smplRate),
							_frameLength(frameLength),
							_smplPerFrame(0),
							_smplPerSeg(0),
							_nbSegmentPerFrame(0),
40
41
							_micHistoryLength(0),
							_spkrHistoryLength(0),
42
43
44
							_alpha(0.01),
							_spkrLevelMem(0),
							_micLevelMem(0),
45
46
47
48
49
							_spkrLevel(0),
							_micLevel(0),
							_spkrHistCnt(0),
							_micHistCnt(0),
							_amplFactor(0.0),
50
51
52
53
54
55
56
57
58
59
							_lastAmplFactor(0.0),
							_amplDelayIndexIn(0),
							_amplDelayIndexOut(0),
							_adaptDone(false),
							_adaptStarted(false),
							_adaptCnt(0),
							_spkrAdaptCnt(0),
							_micAdaptCnt(0),
							_spkrAdaptSize(SPKR_ADAPT_SIZE),
							_micAdaptSize(MIC_ADAPT_SIZE),
60
							_correlationSize(0),
61
62
63
							_processedByte(0),
							_echoActive(true),
							_noiseActive(true)
64
65
66
{
  _debug("EchoCancel: Instantiate echo canceller");

67
 
68
69
70
  micFile = new ofstream("micData", ofstream::binary);
  echoFile = new ofstream("echoData", ofstream::binary);
  spkrFile = new ofstream("spkrData", ofstream::binary);
71

72
  micLevelData = new ofstream("micLevelData", ofstream::binary);
73
  spkrLevelData = new ofstream("spkrLevelData", ofstream::binary);
74

75

76
77
  _micData = new RingBuffer(50000);
  _spkrData = new RingBuffer(50000);
78
  _spkrDataOut = new RingBuffer(50000);
79
80
81

  _micData->createReadPointer();
  _spkrData->createReadPointer();
82
  _spkrDataOut->createReadPointer();
83

84
  // variable used to sync mic and spkr
85
86
  _spkrStoped = true;

87
  _smplPerFrame = (_samplingRate * _frameLength) / MS_PER_SEC;
88
  _smplPerSeg = (_samplingRate * SEGMENT_LENGTH) / MS_PER_SEC;
89
90
  _micHistoryLength = MIC_LENGTH / SEGMENT_LENGTH;
  _spkrHistoryLength = SPKR_LENGTH / SEGMENT_LENGTH;
91
  _nbSegmentPerFrame =  _frameLength / SEGMENT_LENGTH;
92

93
#ifdef HAVE_SPEEXDSP_LIB
94
  _noiseState = speex_preprocess_state_init(_smplPerFrame, _samplingRate);
95
  int i=1;
96
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DENOISE, &i);
97
98
  i=-30;
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_NOISE_SUPPRESS, &i);
99
  i=0;
100
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_AGC, &i);
101
  i=8000;
102
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_AGC_LEVEL, &i);
103
  i=0;
104
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DEREVERB, &i);
105
  float f=0.0;
106
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DEREVERB_DECAY, &f);
107
  f=0.0;
108
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DEREVERB_LEVEL, &f);
109
#endif
110

111
112
113
  memset(_avgSpkrLevelHist, 0, BUFF_SIZE*sizeof(int));
  memset(_avgMicLevelHist, 0, BUFF_SIZE*sizeof(int));

114
  memset(_delayLineAmplify, 0, MAX_DELAY_LINE_AMPL*sizeof(float));
115

116
  _amplDelayIndexIn = 0;
117
118
119
  _amplDelayIndexOut = 0;

  _correlationSize = _spkrAdaptSize;
120
121
122
123
124
125
126
127
128
129
130
131
132
  
}

EchoCancel::~EchoCancel() 
{
  _debug("EchoCancel: Delete echo canceller");

  delete _micData;
  _micData = NULL;

  delete _spkrData;
  _spkrData = NULL;

133
#ifdef HAVE_SPEEXDSP_LIB
134
  speex_preprocess_state_destroy(_noiseState);
135
#endif
136
  
137
138
139
  micFile->close();
  spkrFile->close();
  echoFile->close();
140

141
142
143
  delete micFile;
  delete spkrFile;
  delete echoFile;
144
145
146
147
148

  micLevelData->close();
  spkrLevelData->close();
  delete micLevelData;
  delete spkrLevelData;
149
  
150

151

152
153
}

154
155
void EchoCancel::reset()
{
156
157
158
159
  _debug("EchoCancel: Reset internal state, Sampling rate %d, Frame size %d", _samplingRate, _smplPerFrame);
  
  memset(_avgSpkrLevelHist, 0, BUFF_SIZE*sizeof(int));
  memset(_avgMicLevelHist, 0, BUFF_SIZE*sizeof(int));
160
161
162
163
164
165
166

  _spkrLevel = 0;
  _micLevel = 0;
  _spkrHistCnt = 0;
  _micHistCnt = 0;
  _amplFactor = 0.0;
  _lastAmplFactor = 0.0;
167

168
169
  _smplPerFrame = (_samplingRate * _frameLength) / MS_PER_SEC;
  _smplPerSeg = (_samplingRate * SEGMENT_LENGTH) / MS_PER_SEC;
170
171
  _micHistoryLength = MIC_LENGTH / SEGMENT_LENGTH;
  _spkrHistoryLength = SPKR_LENGTH / SEGMENT_LENGTH;
172
173
  _nbSegmentPerFrame =  _frameLength / SEGMENT_LENGTH;

174
  memset(_delayLineAmplify, 0, MAX_DELAY_LINE_AMPL*sizeof(float));
175

176
  _amplDelayIndexIn = 0;
177
  _amplDelayIndexOut = ECHO_LENGTH / SEGMENT_LENGTH;
178
179
180
181
182
183

  _adaptDone = false;
  _adaptStarted = false;
  _adaptCnt = 0;
  _spkrAdaptCnt = 0;
  _micAdaptCnt = 0;
184
185
186

  _micData->flushAll();
  _spkrData->flushAll();
187
  _spkrDataOut->flushAll();
188

189
#ifdef HAVE_SPEEXDSP_LIB
190
191
192
193
194
  speex_preprocess_state_destroy(_noiseState);

  _noiseState = speex_preprocess_state_init(_smplPerFrame, _samplingRate);
  int i=1;
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DENOISE, &i);
195
196
  i=-30;
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_NOISE_SUPPRESS, &i);
197
198
199
200
201
202
  i=0;
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_AGC, &i);
  i=8000;
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_AGC_LEVEL, &i);
  i=0;
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DEREVERB, &i);
203
  float f=0.0;
204
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DEREVERB_DECAY, &f);
205
  f=0.0;
206
  speex_preprocess_ctl(_noiseState, SPEEX_PREPROCESS_SET_DEREVERB_LEVEL, &f);
207
#endif
208
209

  _spkrStoped = true;
210
211

  _processedByte = 0;
212
213
}

214
215
void EchoCancel::putData(SFLDataFormat *inputData, int nbBytes) 
{
216

217
  if(_spkrStoped) {
218
      _debug("EchoCancel: Flush data");
219
220
221
222
223
224
225
226
227
      _micData->flushAll();
      _spkrData->flushAll();
      _spkrStoped = false;
  }

  // Put data in speaker ring buffer
  _spkrData->Put(inputData, nbBytes);
}

228
229
230
231
232
233
234
235
236
237
238
239
240
int EchoCancel::getData(SFLDataFormat *outputData)
{

  int copied = 0;

  if(_processedByte > 0) {
       copied = _spkrDataOut->Get(outputData, _processedByte);
       _processedByte = 0;
  }

   return copied;
}

241
242
243
244
void EchoCancel::process(SFLDataFormat *data, int nbBytes) {}

int EchoCancel::process(SFLDataFormat *inputData, SFLDataFormat *outputData, int nbBytes)
{
245

246
  if(_spkrStoped) {
247
248
      bcopy(inputData, outputData, nbBytes);
      return nbBytes;
249
  }
250

251
  int byteSize = _smplPerFrame*sizeof(SFLDataFormat);
252
253

  // init temporary buffers
254
255
256
  memset(_tmpSpkr, 0, BUFF_SIZE*sizeof(SFLDataFormat));
  memset(_tmpMic, 0, BUFF_SIZE*sizeof(SFLDataFormat));
  memset(_tmpOut, 0, BUFF_SIZE*sizeof(SFLDataFormat));
257
258
259
260
261
262
263
264

  // Put mic data in ringbuffer
  _micData->Put(inputData, nbBytes);

  // Store data for synchronization
  int spkrAvail = _spkrData->AvailForGet();
  int micAvail = _micData->AvailForGet();

265
  // _debug("EchoCancel: speaker avail %d, mic avail %d, processed: %d", spkrAvail/320, micAvail/320, _processedByte/320);
266

267
268
269
  // Init number of frame processed
  int nbFrame = 0;

270
  // Get data from mic and speaker internal buffer
271
  while((spkrAvail >= byteSize) && (micAvail >= byteSize)) {
272

273
274
275
    // get synchronized data
    _spkrData->Get(_tmpSpkr, byteSize);
    _micData->Get(_tmpMic, byteSize);
276
    
277
278
    micFile->write((const char *)_tmpMic, byteSize);
    spkrFile->write((const char *)_tmpSpkr, byteSize);
279
280

#ifdef HAVE_SPEEXDSP_LIB  
281
    // Remove noise
282
283
    if(_noiseActive)
        speex_preprocess_run(_noiseState, _tmpMic);
284
#endif
285

286
287
288
    // Processed echo cancellation
    performEchoCancel(_tmpMic, _tmpSpkr, _tmpOut);

289
    echoFile->write((const char *)_tmpOut, byteSize);
290

291
    bcopy(_tmpOut, outputData+(nbFrame*_smplPerFrame), byteSize);
292

293
294
295
    // used to sync with speaker 
    _processedByte += byteSize;

296
297
298
299
300
301
302
    spkrAvail = _spkrData->AvailForGet();
    micAvail = _micData->AvailForGet();

    // increment nb of frame processed
    ++nbFrame;
  }

303
  return nbFrame * _smplPerFrame;
304
305
306
307
308
309
}

void EchoCancel::process(SFLDataFormat *micData, SFLDataFormat *spkrData, SFLDataFormat *outputData, int nbBytes){

}

310
311
312
313
314
315
316
317
318
void EchoCancel::setSamplingRate(int smplRate) {
  
  if (smplRate != _samplingRate) {
    _samplingRate = smplRate;

    reset();
  }
}

319
320
321

void EchoCancel::performEchoCancel(SFLDataFormat *micData, SFLDataFormat *spkrData, SFLDataFormat *outputData) {

322
  for(int k = 0; k < _nbSegmentPerFrame; k++) {
323

324
325
    updateEchoCancel(micData+(k*_smplPerSeg), spkrData+(k*_smplPerSeg));

326
    _spkrLevel = getMaxAmplitude(_avgSpkrLevelHist, _spkrHistoryLength);
327
    _micLevel = getMaxAmplitude(_avgMicLevelHist, _micHistoryLength)/6;
328
329
330

    // _debug("_spkrLevel: (max): %d", _spkrLevel);
    // _debug("_micLevel: (min): %d", _micLevel);
331

332
    if(_spkrLevel >= MIN_SIG_LEVEL) {
333
334
335
336
337
338
339
        if(_micLevel > _spkrLevel) {
	  increaseFactor(0.2);
	  // _amplFactor = 0.0;
	}
	else {
	    _amplFactor = 0.0;
	}
340
341
    }
    else {
342
      increaseFactor(0.01);
343
    }
344

345
346
    // lowpass filtering
    float amplify = (_lastAmplFactor + _amplFactor) / 2;
347
    _lastAmplFactor = _amplFactor;
348

349
350
    if(!_echoActive)
        amplify = 1.0;
351

352
    amplifySignal(micData+(k*_smplPerSeg), outputData+(k*_smplPerSeg), amplify);
353
354
    
  }
355

356
357
358
359
360
}


void EchoCancel::updateEchoCancel(SFLDataFormat *micData, SFLDataFormat *spkrData) {

361
  // TODO: we should find a way to normalize signal at this point
362
  int micLvl = computeAmplitudeLevel(micData, _smplPerSeg);
363
  int spkrLvl = computeAmplitudeLevel(spkrData, _smplPerSeg);
364

365
366
367
368
369
370
  SFLDataFormat tempSpkrLevel[_smplPerSeg];
  SFLDataFormat tempMicLevel[_smplPerSeg];

  _spkrLevelMem = estimatePower(spkrData, tempSpkrLevel, _smplPerSeg, _spkrLevelMem);
  _micLevelMem = estimatePower(micData, tempMicLevel, _smplPerSeg, _micLevelMem);

371
372
373
  // Add 1 to make sure we are not dividing by 0
  _avgMicLevelHist[_micHistCnt++] = micLvl+1;
  _avgSpkrLevelHist[_spkrHistCnt++] = spkrLvl+1;
374
375
376
377
378

  // _debug("micLevel: %d", micLvl);
  // _debug("spkrLevel: %d", spkrLvl);

  if(_micHistCnt >= _micHistoryLength)
379
380
    _micHistCnt = 0;

381
  if(_spkrHistCnt >= _spkrHistoryLength)
382
    _spkrHistCnt = 0;
383
384
385
386


  micLevelData->write((const char*)tempMicLevel, sizeof(SFLDataFormat)*_smplPerSeg);
  spkrLevelData->write((const char*)tempSpkrLevel, sizeof(SFLDataFormat)*_smplPerSeg);
387
  /*
388
  // if adaptation done, stop here
389
390
  // if(_adaptDone)
  if(true)
391
392
393
394
395
396
397
398
399
    return;

  // start learning only if there is data played through speaker
  if(!_adaptStarted) {
    if(spkrLvl > MIN_SIG_LEVEL)
      _adaptStarted = true;
    else
      return;
  }
400

401
402
403
404
405
406
407
408
409
  if(_spkrAdaptCnt < _spkrAdaptSize)
      _spkrAdaptArray[_spkrAdaptCnt++] = spkrLvl;

  if(_micAdaptCnt < _micAdaptSize)
      _micAdaptArray[_micAdaptCnt++] = micLvl;

  // perform correlation if spkr size is reached
  if(_adaptCnt > _spkrAdaptSize) {
      int k = _adaptCnt - _spkrAdaptSize;
410
      _correlationArray[k] = performCorrelation(_spkrAdaptArray, _micAdaptArray+k, _correlationSize); 
411
      // _debug("EchoCancel: Correlation: %d", _correlationArray[k]);
412
413
414
415
416
417
418
419
  }

  _adaptCnt++;

  // if we captured a full echo
  if(_adaptCnt == _micAdaptSize) {
    _debug("EchoCancel: Echo path adaptation completed");
    _adaptDone = true;
420
    _amplDelayIndexOut = 0;// getMaximumIndex(_correlationArray, _correlationSize);
421
    _debug("EchoCancel: Echo length %d", _amplDelayIndexOut);
422
  }
423
  */
424
425
426
}


427
int EchoCancel::computeAmplitudeLevel(SFLDataFormat *data, int size) {
428

429
430
  int level = 0;

431
  for(int i = 0; i < size; i++) {
432
433
434
435
436
437
    if(data[i] >= 0.0)
      level += (int)data[i];
    else
      level -= (int)data[i];
  }

438
  level = level / _smplPerSeg;
439
440

  return level;
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
  
}

SFLDataFormat EchoCancel::estimatePower(SFLDataFormat *data, SFLDataFormat *ampl, int size, SFLDataFormat mem) {

  float memFactor = 1.0 - _alpha;

  for (int i = 0; i < size; i++) {
    mem = (SFLDataFormat)(memFactor*(float)mem + abs(_alpha*(float)data[i]));
    // _debug("ampl: %d, memfactor: %f, alpha: %f, data: %d", mem, memFactor, _alpha, data[i]);
    ampl[i] = mem;
  }

  return mem;

456
457
458
}


459
int EchoCancel::getMaxAmplitude(int *data, int size) {
460
461
462

  SFLDataFormat level = 0.0;

463
  for(int i = 0; i < size; i++) {
464
465
466
467
468
469
470
471
    if(data[i] >= level)
      level = data[i];
  }

  return (int)level;
}


472
void EchoCancel::amplifySignal(SFLDataFormat *micData, SFLDataFormat *outputData, float amplify) {
473

474
475
  // for(int i = 0; i < _smplPerSeg; i++)
  //  outputData[i] = micData[i];
476
477

  // Use delayed amplification factor due to sound card latency 
478
  // do not increment amplitude array if adaptation is not done 
479
480
  // if (_adaptDone) {
  if (true) {
481
482
483
    for(int i = 0; i < _smplPerSeg; i++) {
        outputData[i] = (SFLDataFormat)(((float)micData[i])*_delayLineAmplify[_amplDelayIndexOut]);
    }
484
485
    _amplDelayIndexOut++;
    _delayLineAmplify[_amplDelayIndexIn++] = amplify;
486
487
488
489
490
491
492
493
  }
  else {
    for(int i = 0; i < _smplPerSeg; i++) {
        outputData[i] = micData[i];
    }
    return;
  }

494
495
  if(_amplDelayIndexOut >= MAX_DELAY_LINE_AMPL)
    _amplDelayIndexOut = 0;
496

497
498
  if(_amplDelayIndexIn >= MAX_DELAY_LINE_AMPL)
    _amplDelayIndexIn = 0;
499

500
}
501
502
503



504
void EchoCancel::increaseFactor(float factor) {
505

506
507
  // Get 200 ms to get back to full amplitude
  _amplFactor += factor;
508
509
510
511
512
513
514
515
516

  if(_amplFactor > 1.0)
    _amplFactor = 1.0;

}


void EchoCancel::decreaseFactor() {

517
  // Takes about 50 ms to react
518
519
520
521
522
  _amplFactor -= 0.2;

  if(_amplFactor < 0.0)
    _amplFactor = 0.0;
}
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550


int EchoCancel::performCorrelation(int *data1, int *data2, int size) {

  int correlation = 0;
  while(size) {
    size--;
    correlation += data1[size] * data2[size];
  }
  return correlation;
}


int EchoCancel::getMaximumIndex(int *data, int size) {

  int index = size;
  int max = data[size-1];
 
  while(size) {
    size--;
    if(data[size] > max) {
        index = size;
	max = data[size];
    }
  }

  return index;
}