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

 //                        A small demo of sonar.
 // Program allow distance measuring.
 // Uses cross-correlation algorithm to find echos
 //
 // Author: kaklik  (kaklik@mlab.cz)
-//
+//$Id:$
 ///////////////////////////////////////////////////////////////////////////////////
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.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
-#define RESOLUTION	100	// resolution per map pixel
 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 */
     int err;
     snd_pcm_hw_params_t *hwparams;
     snd_pcm_sw_params_t *swparams;
     long int *correlationl, *correlationr;
+    float *echo_map;
     int *L_signal, *R_signal;
     short *chirp, *signal;
     float *chirp_spect, *lecho_spect, *recho_spect;
     float x,y;
     unsigned int i,j,m,n;
     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;
     snd_pcm_hw_params_alloca(&hwparams);
     snd_pcm_sw_params_alloca(&swparams);
-    printf("Simple PC sonar ver. 000000001 starting work.. \n");
+    printf("Simple PC sonar $Rev:$ starting work.. \n");
 //open and set playback device
     if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)
+    {
         printf("Playback open error: %s\n", snd_strerror(err));
     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));
     chirp = calloc(2*period_size, sizeof(short));
     signal = malloc(2*period_size * sizeof(short));
+    echo_map = malloc(3*period_size * sizeof(float));   // Array to store two dimensional image of echos
 // generate ping pattern
     chirp_size = linear_windowed_chirp(chirp);
     frequency_bins = chirp_size / 2 + 1;
+        }
         correlationl[n]=abs(l);
         correlationr[n]=abs(r);
+    }
+    m=0;
     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++)
+	for (i=0;i < period_size; i++)
+	{
-	  x=(float)i*RESOLUTION; y=(float)j*RESOLUTION;	 //transofm integger index of array to float with appproopirate resolution
+		for(j=0;j < period_size; j++)
+		{
+			echo_map[m]=(i*i-j*j+APERTURE*APERTURE)/(2*APERTURE);
-	  density_map[i][j]=(float)correlationl[(int)sqrt(x*x + y*y)]*correlationr[(int)sqrt(APERTURE*APERTURE - 2*APERTURE*x + x*x + y*y)];
+			echo_map[m+1]=sqrt(-(i-j-APERTURE)*(i+j-APERTURE)*(i-j+APERTURE)*(i+j+APERTURE))/(2*r);
+			echo_map[m+2]=correlationl[i]*correlationr[j];
+			m+=3;
+		}
+	}
-    }
     printf("Searching echos\n");
     r=0;
     l=0;
             delayr[1] = n;
             r = correlationr[n];
+        }
+    }
-//spocitejj frekvencni spektrum pro levy kanal
+//spocitej frekvencni spektrum pro levy kanal
     for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];
     fftw_execute(fft_plan_chirp);
     for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
+    {
         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);
+    j=0;
     out=fopen("/tmp/plane_cut.txt","w"); // writes plane cut - e.g. density map to file
-    for (i=0;i < MAP_SIZE; i++)
+    for (i=0;i < period_size; i++)
+    {
-	for (j=0;j < MAP_SIZE; j++) fprintf(out,"%3.2f ", density_map);
+	fprintf(out,"%3.2f %3.2f %3.2f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);
-	fprintf(out,"\n");
+	j+=3;
+    }
     out=fopen("/tmp/chirp.txt","w");
     for (i=0; i <= (chirp_size - 1); i++)
+    {
     free(correlationr);
     free(L_signal);
     free(R_signal);
     free(chirp);
     free(signal);
+    free(echo_map);
     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 645 → 646