WebSVN – svnkaklik – Diff – /programy/C/ix86/echo/SW/sonar/src/sonar.c

 #include <errno.h>
 #include <getopt.h>
 #include <alsa/asoundlib.h>
 #include <sys/time.h>
 #include <math.h>
+#include <fftw3.h>
 #define SOUND_SPEED	340.0	// sound speed in air in metrs per second
 #define MAX_RANGE	10.0	// maximal working radius in meters
 static char *device = "plughw:0,0";			/* playback device */
     if (err < 0)
+    {
         printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));
         return err;
+    }
     // start the transfer when the buffer is almost full: never fou our case
     err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);
     if (err < 0)
+    {
         printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));
         return err;
     snd_pcm_sw_params_t *swparams;
     long int *correlationl, *correlationr;
     int *L_signal, *R_signal;
     short *chirp, *signal;
+    float *chirp_spect, *echo_spect;
     unsigned int i,j,m,n;
-    unsigned int delay[10];	//store delay of signifed correlation
+    unsigned int delayl[10],delayr[10];	//store delay of signifed correlation
     long int l,r;  // store correlation at strict time
+    double df;	//frequency resolution
+    unsigned int frequency_bins; // number of output frequency bins
+    double *inchirp;
+    fftw_complex *outchirp;
+    fftw_plan fft_plan_chirp;
     FILE *out;
     snd_pcm_hw_params_alloca(&hwparams);
     snd_pcm_sw_params_alloca(&swparams);
+    {
         printf("Setting of swparams failed: %s\n", snd_strerror(err));
         exit(EXIT_FAILURE);
+    }
-/*    err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together
+    /*    err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together
-    if (err < 0)
+        if (err < 0)
-    {
+        {
-        printf("Device linking error: %s\n", snd_strerror(err));
+            printf("Device linking error: %s\n", snd_strerror(err));
-        exit(EXIT_FAILURE);
+            exit(EXIT_FAILURE);
-    }*/
+        }*/
     correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve
     correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve
     L_signal = malloc(period_size * sizeof(int));
     R_signal = malloc(period_size * sizeof(int));
     signal = malloc(2*period_size * sizeof(short));
 // generate ping pattern
     chirp_size = linear_windowed_chirp(chirp);
+    frequency_bins = chirp_size / 2 + 1;
+    df = (double) rate / (double) chirp_size;
+    chirp_spect = malloc(frequency_bins * sizeof(float));
+    echo_spect = malloc(frequency_bins * sizeof(float));
+    inchirp = fftw_malloc(sizeof(double) * chirp_size); 		// allocate input array for FFT
+    outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);
+    fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);
+    printf("compute chirp spectrum\n");
+    for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];
+    fftw_execute(fft_plan_chirp);
+    for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );
+// write chirp data to souncard buffer
     err = snd_pcm_writei(playback_handle, chirp, period_size);
     if (err < 0)
+    {
         printf("Initial write error: %s\n", snd_strerror(err));
         exit(EXIT_FAILURE);
         printf("Start error: %s\n", snd_strerror(err));
         exit(EXIT_FAILURE);
+    }
     else printf("Waiting for transmitt all samples\n");
 //--------------
     while ( snd_pcm_avail_update(capture_handle) < period_size)
+    {
         usleep(1000);
         printf(".");
+    }
     printf("\nData transmitted \ncorrelating\n");
     for (n=0; n < (period_size - chirp_size - 1); n++)
+    {
         l=0;
         r=0;
-        for (m=0;m < chirp_size;m++)
+        for ( m = 0; m < chirp_size;m++)
+        {
             l += chirp[m]*L_signal[m+n];	// correlate with left channel
             r += chirp[m]*R_signal[m+n];	// correlate with right channel
+        }
-        correlationl[n]=l;
+        correlationl[n]=abs(l);
-        correlationr[n]=r;
+        correlationr[n]=abs(r);
+    }
     printf("Searching echos\n");
     r=0;
     l=0;
     for (n=0; n < period_size;n++) 			//najde nejvetsi korelace
+    {
         if (l < correlationl[n])
+        {
-            delay[1] = n;
+            delayl[1] = n;
             l = correlationl[n];
+        }
         if (r < correlationr[n])
+        {
-            delay[2] = n;
+            delayr[1] = n;
             r = correlationr[n];
+        }
+    }
     printf("Writing output file\n");
     out=fopen("/tmp/sonar.txt","w");
     j=0;
-    for (i=0; i <= (period_size -1); i++)
+    for (i=0; i <= (period_size - 1); i++)
+    {
+        fprintf(out,"%6d %6d %6d %9ld %9ld\n",i,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
+        j+=2;
+    }
+    fclose(out);
+    out=fopen("/tmp/chirp.txt","w");
+    j=0;
+    for (i=0; i <= (chirp_size - 1); i++)
+    {
-        fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
+        fprintf(out,"%6d %6d %f\n", i, chirp[i], chirp_spect[i]);
         j+=2;
+    }
     fclose(out);
-    printf("Echo zacina na: %d vzorku.\n", delay[1]);
+    printf("Echo zacina na: %d vzorku.\n", delayl[1]);
-    printf("Casove na: %f s\n", ((float)delay[1]/rate));
+    printf("Casove na: %f s\n", ((float)delayl[1]/rate));
-    printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delay[1]/rate));
+    printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delayl[1]/rate));
+    free(correlationl);
+    free(correlationr);
+    free(L_signal);
+    free(R_signal);
+    free(chirp);
+    free(signal);
     snd_pcm_close(playback_handle);
     snd_pcm_close(capture_handle);
     return 0;
+}

Subversion Repositories svnkaklik

(root)/programy/C/ix86/echo/SW/sonar/src/sonar.c – Rev 562 → 563