timedelay.cpp

Go to the documentation of this file.

#include <cmath>
#include <iostream>
#include "timedelay.h"
#include <libraries/Fft/Fft.h>
using std::cerr;
using std::endl;
 
TimeDelay::TimeDelay(const float& fs):
  fs(fs)
{
 
  static_assert(BLOCKSIZE > 2);
  // Initialize copy arrays to zero
  mic1_copy.resize(2*BLOCKSIZE, 0.0f);
  mic2_copy.resize(2*BLOCKSIZE, 0.0f);
  hann.resize(BLOCKSIZE);
 
  // Create a Hann Window
  for(us i=0;i<BLOCKSIZE;i++) {
    float phase = 2*M_PI*i/(BLOCKSIZE-1);
    hann[i] = 0.5f*(1.0-cosf(phase));
  }
  fft1 = new Fft();
  fft2 = new Fft();
  ifft = new Fft();
 
  fft1->setup(2*BLOCKSIZE);
  fft2->setup(2*BLOCKSIZE);
  ifft->setup(2*BLOCKSIZE);
 
  re_conv.resize(BLOCKSIZE+1, 0.0f);
  im_conv.resize(BLOCKSIZE+1, 0.0f);
 
 
}
 
float TimeDelay::determineDelay(const Channel& mic1,
    const Channel& mic2){
 
  // Multiply with Window and add to block
  /* const us offset = BLOCKSIZE/2; */
 
  for(us i=0;i<BLOCKSIZE;i++) {
    mic1_copy[i] = mic1.at(i)*hann.at(i);
    mic2_copy[i] = mic2.at(i)*hann.at(i);
  }
  std::fill(mic1_copy.begin()+BLOCKSIZE,mic1_copy.end(), 0);
  std::fill(mic2_copy.begin()+BLOCKSIZE,mic2_copy.end(), 0);
 
  fft1->fft(mic1_copy);
  fft2->fft(mic2_copy);
 
  // Multiply one by the other in frequency domain, write back to first part of
  // both arrays: mic1_copy is used as the real part of that signal, and mic2
  // as the imaginary part of the signal.
  for(us i=0;i<BLOCKSIZE+1;i++) {
    /* for(us i=0;i<BLOCKSIZE;i++) { */
    /* cerr << "i: " << i <<",Mic 1 copy [i]: " <<mic1_copy.at(i) << endl; */
    /* cerr << "i: " << i <<",Mic 2 copy [i]: " <<mic2_copy.at(i) << endl; */
 
    // Note: in the frequency domain, a cross-correlation denotes the complex
    // conjugate of f times g: f x g.
    // Splitting up:
    // re(f x g) = re(f) * re(g) - im(f) * im(g)
    // im(f x g) = im(f) * re(g) + re(f) * im(g)
 
    // Real part multiplication, yes a plus there
    re_conv.at(i) = fft1->fdr(i)*fft2->fdr(i) - fft1->fdi(i)*fft2->fdi(i);
 
    // Imaginary part multiplication
    im_conv.at(i) = fft1->fdi(i)*fft2->fdr(i) + fft1->fdr(i)*fft2->fdi(i);
 
    /* cerr << "i: " << i << ". abs: "<< fft1.fda(i) << endl; */
    /* cerr << "i: " << i << ". abs: "<< fft2.fda(i) << endl; */
  }
 
  // Perform IFFT
  ifft->ifft(re_conv, im_conv);
  float max_val = 0.0f;
  /* float max_val = -1e10f; */
  int arg = 0;
 
  // Determine maxabs index
  for(us i=0;i<2*BLOCKSIZE;i++) {
    float curval = ifft->td(i);
    /* cerr << "i: " << i << "curval: " << curval << endl; */
    float abscurval = fabsf_neon(curval);
 
    if(abscurval > max_val) {
      /* if(curval > max_val) { */
      max_val = abscurval;
      arg = i;
      /* cerr << "i: " << i << "New max val: " << max_val << endl; */
    }
    /* cerr << i << endl; */
    }
 
    /* cerr << "val: " << ifft.td(arg) << endl; */ 
    // If in the high end, it is wrapped around to the negative part, due to
    // the cyclic nature of the FFT and we have a negative value
    // instead.
    /* cerr << "arg without adjustment: " << arg << endl; */ 
    if(arg > BLOCKSIZE/2) {
      arg = -(BLOCKSIZE - arg);
    }
    /* cerr << "arg: " << arg << endl; */ 
    return 1000*float(arg)/fs;
 
  }
 
  TimeDelay::~TimeDelay() {
    /* delete convolver; */
    delete fft1;
    delete fft2;
    delete ifft;
 
  }