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

 #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
+#define APERTURE	0.2	// distance between microphones
+#define MAP_SIZE	100
 static char *device = "plughw:0,0";			/* playback device */
 static snd_pcm_format_t format = SND_PCM_FORMAT_S16;	/* sample format */
 static unsigned int rate = 96000;			/* stream rate */
 static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6);		/* ring buffer length in us */
     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
+    float density_map[MAP_SIZE][MAP_SIZE];  // Array to store two dimensional image of echos
     double *inchirp;
     fftw_complex *outchirp;
     fftw_plan fft_plan_chirp;
     FILE *out;
+        }
         correlationl[n]=abs(l);
         correlationr[n]=abs(r);
+    }
+    printf("Building echo map\n");		// compute map from left and right correlation data
+    for (i=0;i < MAP_SIZE; i++)
+    {
+	for (j=0;j < MAP_SIZE; j++) density_map[i][j]=correlationl[(int)sqrt(i*i + j*j)]*correlationr[(int)sqrt(APERTURE*APERTURE - 2*APERTURE*i + i*i + j*j)];
+    }
     printf("Searching echos\n");
     r=0;
     l=0;
     for (n=0; n < period_size;n++) 			//najde nejvetsi korelace
+    {
+    {
         fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",SOUND_SPEED * (float) i / rate,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
+    }
     fclose(out);
+    out=fopen("/tmp/plane_cut.txt","w");
+    for (i=0;i < MAP_SIZE; i++)
+    {
+	for (j=0;j < MAP_SIZE; j++) fprintf(out,"%6f", density_map);
+    }
     out=fopen("/tmp/chirp.txt","w");
     for (i=0; i <= (chirp_size - 1); i++)
+    {
         fprintf(out,"%6d %6d\n", i, chirp[i]);
+    }
+    {
         fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);
+    }
     fclose(out);
-    printf("Echo zacina na: %d vzorku.\n", delayl[1]);
-    printf("Casove na: %f s\n", ((float)delayl[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);

Subversion Repositories svnkaklik

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