Hello All,
So i've been able to make little progress on this project. To test the echo cancellation algorithm i've chosen to use simple NLMS algorithm. the way i'm testing this algorithm is i'm running the set up on one teensy and sending mic signal to both inputs of NLMS algorithm, far side and near side. The code works, but the problem i'm having is that I'm not sure how to make it faster, or keep up with teensy's sampling. I've posted code bellow.
Basically, because of 'for' loops the NLMS algorithm takes long to process one block of audio samples. 1 block is about 2.9 ms of time, but running code in real time takes longer. As a result, the output to speaker is choppy because NLMS code has to complete before audio starts sampling again. I'm looking for some advice on how to implement it smarter, or mayby i'm approaching this wrong way. Any advice would be greatly appreciated.
Code:
//
// Microphone to Speaker output
#define NUMEL(x) (sizeof(x)/sizeof((x)[0])) // retuns/calculates the size of an array
//#include "NLMS.h"
#include <Audio.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <SerialFlash.h>
//*****functions**********
void getBuffer1();
void playData();
void displayData();
void NLMS_AEC();
// GUItool: begin automatically generated code
AudioInputI2S Mic; //microphone
AudioPlayQueue MicMem; //queue for microphone
AudioRecordQueue Memory;
AudioOutputI2S headphones; //output
AudioConnection patchCord1(Mic, 0, Memory, 0);
AudioConnection patchCord2(MicMem, 0, headphones,0);
AudioConnection patchCord3(MicMem, 0, headphones,1);
AudioControlSGTL5000 sgtl5000_1; //xy=339.6666564941406,285.6666488647461
const int numBlocks = 2; //min block size is 2!!!!!!
const int lFilt = numBlocks-1;
const int gg = lFilt*128;
int16_t *pAddr; //pointer to 128 block of getBuffer
int16_t lblock = numBlocks*128; //length of arrays
int16_t ablock[128*numBlocks]; // set an array block
int16_t *pablock; // pointer to an array block
int16_t *pp; // pointer to getBuffer
int n;
unsigned long time1; // time variable
//***************NMLS const and settings*************************
const int16_t numTaps = gg;
float mu = 0.0001;
float mu0 = 0;
float psi = 0.0001;
float w[numTaps];
float *pw; // pointer to w
float yhat = 0;
float xtdl = 0;
//define input/source
const int micInput = AUDIO_INPUT_MIC;
void setup() {
memset(w,0,numTaps);
memset(ablock,0,128*numBlocks);
//initialize pointers for NLMS
pw = w;
Serial.begin(115200); // initiate baud rate for serial comm.
AudioMemory(8); // allocate # of audio memory (each block has 128 sample of data)
sgtl5000_1.enable(); // enalbe
sgtl5000_1.volume(0.5); // output volume
sgtl5000_1.inputSelect(micInput); //input selection (mic or line in)
sgtl5000_1.micGain(36); // mic gain
//set array values to zero
pablock = ablock; // assign pointer to array
delay(1000);
}
//***************************MAIN LOOP********************************************************************
void loop() {
getBuffer1(numBlocks); //returns pointer to the buffered data
//print sample to serial plotter
//displayData(pablock); // print data to serial plotter
NLMS_AEC(pablock,pablock,pablock,pw,gg,.0001);
}
//*********************************************************************************************************
// print data to serial plotter
void displayData(int16_t * data){
for(int i = 0; i<128*numBlocks;i++){
Serial.println(*(data+i));
}
}
// play data to speaker
void playData(int16_t *data, int i){
pAddr = MicMem.getBuffer(); //get pointer to 128 array playBuffer
// take one block from buffer array (ablock) and copy it to playBuffer pointer
// play that block
memcpy((byte*)pAddr,(byte*)data+(256*i),256);
MicMem.playBuffer();
}
// buffer an array with samples from Microphone
// function receives and array and integer indicating number of blocks
void getBuffer1(int x) {
Memory.begin(); // begin putting sample data into memory
int l = 0;
memcpy((byte*)pablock,(byte*)pablock+256,256*(x-1));
// put 128 blocks of sample into an array size of 128*(number of blocks)
while(l == 0){ // wait until block samples are available
if(Memory.available() ==1) {// if availabe copy 128 block to array and get out of while loop
//read one 128 block of samples and copy it into array
memcpy((byte*)pablock+(256*(x-1)),Memory.readBuffer(),256);
Memory.freeBuffer(); // free buffer memory
//NLMS_AEC(pablock,pablock,pablock,pw,gg,.0001);
playData(pablock,(x-1)); // play 128 block from buffered array
l = 1; // set n to 1 to get out of while loop
}//end if
}//end while
l = 0;
//Memory.clear(); // clear all audio memory
Memory.end(); // end memory capture
// return ablock; // regurn pointer to array
}
// NMLS algorithm
// inputs are Mic Signal, far end signal and index of n-1 block wher n is the current block itteration
// previous block data is used to modify current block data samples
void NLMS_AEC(int16_t *Mic, int16_t *x){
for(int h = 0; h<128;h++){
for(int j = gg; j >0;j--){
yhat = yhat+x[j+h]*pw[gg-j];
}
pablock[h+gg] = (int16_t)(Mic[gg+h]-yhat);
yhat = 0;
for(int j = gg; j >0;j--){
xtdl = xtdl+x[j+h]*x[j+h];
}
xtdl = xtdl + psi;
mu0 = mu/xtdl;
xtdl = 0;
//update filter taps
for(int j = 0; j<gg;j++){
pw[j] = pw[j] + x[gg-j+h]*mu0*pablock[h+gg];
}//end for
}//end for outer
}// end NLMS_AEC
Reply With Quote
Originally Posted by snirtikam
