PS Eye GCC-PHAT

This was the first step in the project.  A single microphone array was used to calculate the angle of arrival of sound waves every quarter second for five seconds.

Alsa programming based on https://www.linuxjournal.com/article/6735

Code:

#define ALSA_PCM_NEW_HW_PARAMS_API #include <alsa/asoundlib.h> #include <stdio.h> #include <math.h> #include <complex.h> #include <fftw3.h>

int main(int argc, const char* argv[]) {     /* FFTW Setup  */     int num_samples = 250 * 48; //0.25 second * period size (2 bytes/sample)     double *channel_1 = (double*) malloc(sizeof(double) * num_samples);     memset(channel_1, 0, sizeof(double) * num_samples);     double *channel_2 = (double*) malloc(sizeof(double) * num_samples);     memset(channel_2, 0, sizeof(double) * num_samples);

    int nComplex = (num_samples / 2) + 1;     fftw_complex *channel_1_out = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * nComplex);     memset(channel_1_out, 0, sizeof(fftw_complex) * nComplex);         fftw_complex *channel_2_out = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * nComplex);     memset(channel_2_out, 0, sizeof(fftw_complex) * nComplex);

    fftw_plan pa = fftw_plan_dft_r2c_1d(num_samples, channel_1, channel_1_out, FFTW_ESTIMATE);         fftw_plan pb = fftw_plan_dft_r2c_1d(num_samples, channel_2, channel_2_out, FFTW_ESTIMATE);

    int i = 0;     fftw_complex *multiplication_result = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * num_samples);         memset(multiplication_result, 0, sizeof(fftw_complex) * (num_samples));

    fftw_complex *out = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * num_samples);     memset(out, 0, sizeof(fftw_complex) * num_samples);

    fftw_plan px = fftw_plan_dft_1d(num_samples, multiplication_result, out, FFTW_BACKWARD, FFTW_ESTIMATE);

    /* ALSA Setup  */     int rc;     int num_channels = 4;     snd_pcm_t *handle;     rc = snd_pcm_open(&handle, "plughw:1,0", SND_PCM_STREAM_CAPTURE, 0);

    if (rc < 0) {         fprintf(stderr, "unable to open pcm device: %s\n", snd_strerror(rc));         exit(1);     }

    snd_pcm_hw_params_t *params;     snd_pcm_hw_params_alloca(¶ms);     snd_pcm_hw_params_any(handle, params);     /* Interleaved mode */     snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);     /* Singed 16-bit little-endian format  */     snd_pcm_hw_params_set_format(handle, params, SND_PCM_FORMAT_S16_LE);     /* Two channels  */     snd_pcm_hw_params_set_channels(handle, params, num_channels);     /* 48000 bits/second sample rate  */     int dir = 0;     unsigned int sampleRate = 48000;     snd_pcm_hw_params_set_rate_near(handle, params, &sampleRate, &dir);     printf("Chosen approximate set rate from snd_pcm_hw_params_set_rate_near(): %u\n", sampleRate);

    /* Set period size to 12000 frames, each frame is 1ms, we want 0.25 second/  */     snd_pcm_uframes_t frames = 250 * 48;     snd_pcm_hw_params_set_period_size_near(handle, params, &frames, &dir);     printf("Chosen approximate target period size from snd_pcm_hw_params_set_period_size_near(): %lu\n", frames);

    /* Write parameters to driver  */     rc = snd_pcm_hw_params(handle, params);     if (rc < 0) {         fprintf(stderr, "unable to set hw parameters: %s\n", snd_strerror(rc));         exit(1);     }     /* Use a buffer large enough to hold 1 period  */     snd_pcm_hw_params_get_period_size(params, &frames, &dir);     printf("Returned approximate period size in frames from snd_pcm_hw_params_get_period_size(): %lu\n", frames);     int size = frames * 2 * num_channels; // 2 bytes/sample * number of channels     printf("Size of buffer: %d\n", size);     char *buffer = (char *) malloc(size);

    /* We want to loop for 5 seconds  */     unsigned int val = -1;     snd_pcm_hw_params_get_period_time(params, &val, &dir);     printf("Returned approximate period duration in microseconds from snd_pcm_hw_params_get_period_time(): %u\n", val);

    long loops = 5000000 / val;     printf("Number of loops to be run: %ld\n", loops);

    int fd = open("stream3.raw", O_CREAT | O_RDWR | O_TRUNC, S_IRUSR | S_IWUSR);     printf("fd: %d\n", fd);     if (fd == -1) {         fprintf(stderr, "Error number when opening fd: %d\n", errno);         return -1;     }     int fd2 = open("stream3_2ch.raw", O_CREAT | O_RDWR | O_TRUNC, S_IRUSR | S_IWUSR);     printf("fd2: %d\n", fd2);     if (fd2 == -1) {         fprintf(stderr, "Error number when opening fd2: %d\n", errno);     }

    while (loops > 0) {         loops--;         rc = snd_pcm_readi(handle, buffer, frames);

        if (rc == -EPIPE) {             /* EPIPE means overrun */             fprintf(stderr, "overrun occurred\n");             snd_pcm_prepare(handle);         } else if (rc == -EBADFD) {             fprintf(stderr, "PCM is not in the right state\n");         } else if (rc < 0) {             fprintf(stderr, "error from read: %s\n", snd_strerror(rc));         } else if (rc != (int) frames) {             fprintf(stderr, "short read, read %d frames\n", rc);         }

        //At this point, buffer contains the frames from all 4 channels         //Recall that each frame is a set of (4) samples, one from each channel.         //Each sample is 2 bytes, [LSB, MSB]         //Our goal here is to get the first channel and the fourth channel.         int z = 0;         char ch1_ch4_buff[2 * 2 * rc]; // 2 bytes/channel * 2 channels * 48 frames = 192         //size = 48 * num_bytes_per_channel * numchannls = 48 * 2 * 4 = 384         for(z = 0; z < size; z += 8) {             char lsb_ch1 = buffer[z];             char msb_ch1 = buffer[z + 1];             char lsb_ch4 = buffer[z + 6];             char msb_ch4 = buffer[z + 7];             ch1_ch4_buff[z / 2] = lsb_ch1; //LSB of channel 1             ch1_ch4_buff[z / 2 + 1] = msb_ch1; //MSB of channel 1             ch1_ch4_buff[z / 2 + 2] = lsb_ch4; //LSB of channel 4;             ch1_ch4_buff[z / 2 + 3] = msb_ch4; //MSB of channel 4;

            //convert these integer samples to doubles and place them in their arrays             int ch1 = (msb_ch1 << 8) + lsb_ch1;             double d_ch1 = ch1 * 1.0;             channel_1[z / 8] = d_ch1;             int ch2 = (msb_ch4 << 8) + lsb_ch4;             double d_ch2 = ch2 * 1.0;             channel_2[z / 8] = d_ch2;         }         //now, ch1_ch4_buff[] contains just the frames from channels 1 and 4         z = write(fd2, ch1_ch4_buff, 2 * 2 * rc);         rc = write(fd, buffer, size);         if (rc != size) {             fprintf(stderr, "short write, wrote %d bytes\n", rc);         }

        //Now that the samples are written to files, let's compute the GCC-PHAT for the previous 0.25 second         //1.  FFT the 16 bit integer samples for each channel into complex doubles         fftw_execute(pa);             fftw_execute(pb);

        //2.  Multiply (element-wise) the two channels together, having taken the complex conjugate of the fourth         //    Also divide by magnitude at this step, for PHAT weighting         for(i = 0; i < nComplex; i++) {                     fftw_complex mult_result_numerator = channel_1_out[i] * conj(channel_2_out[i]);                     double mult_result_denominator = cabs(channel_1_out[i]) * cabs(conj(channel_2_out[i]));                     multiplication_result[i] = mult_result_numerator / mult_result_denominator;             }

        //3.  Apply inverse transform to the product from step 2.         fftw_execute(px);

        //4.  Rotate output array so that center of array corresponds to broadside angle         double rotated[num_samples];         for(i = 0; i < num_samples; i++){             fftw_complex to_rotate = out[ (i + nComplex) % (num_samples) ] / num_samples;             double abs_to_rotate = cabs(to_rotate);             rotated[i] = abs_to_rotate;         }

        //5.  Get argmax from GCC-PHAT         double max_amp = 0.0;             int max_index = 0;             for(i = 0; i < num_samples; i++){             double amp = rotated[i];             if(amp > max_amp) {                 max_amp = amp;                 max_index = i;             }

            /*                     double amp = cabs(out[i]);                     if(amp > max_amp) {                             max_amp = amp;                             max_index = i;                     }             */             }             printf("Max amplitude of GCC-PHAT occurs at: %d\n", max_index);         /*         double delay = (max_index - (num_samples / 2)) * (1.0/48000);         double sin_theta = (delay * 343.3) / 0.06;         double theta = asin(sin_theta);         printf("delay: %f\n", delay);                 printf("theta (radians): %f\n", theta);                 printf("theta (degrees): %f\n", theta * (180 / M_PI));         */

        if(max_index > num_samples / 2) {             double delay = (max_index - (num_samples / 2)) * (1.0/48000);             double sin_theta = ( delay * 343.3 ) / 0.06;             double theta = asin(sin_theta);             printf("delay: %f\n", delay);             printf("theta (radians): %f\n", theta);             printf("theta (degrees): %f\n", theta * (180 / M_PI));         }         else if(max_index < num_samples / 2) {             double delay = ((num_samples / 2) - max_index) * (1.0/48000);             double sin_theta = ( delay * 343.3 ) / 0.06;             double theta = asin(sin_theta);             printf("delay: %f\n", delay * -1);             printf("theta (radians): %f\n", theta * -1);             printf("theta (degrees): %f\n", (theta * (180 / M_PI)) * -1);         }         else {             printf("delay: %f\n", 0.0);                         printf("theta (radians): %f\n", 0.0);                         printf("theta (degrees): %f\n", 0.0);

        }

    }

    snd_pcm_drain(handle);     snd_pcm_close(handle);     free(buffer);     close(fd);     close(fd2);

    free(channel_1);     free(channel_2);

    fftw_destroy_plan(pa);         fftw_destroy_plan(pb);     fftw_destroy_plan(px);

    fftw_free(channel_1_out);         fftw_free(channel_2_out);     fftw_free(multiplication_result);     fftw_free(out);

    return 0; }