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 Xl	-0.1		// microphones position
+//#define Xl	-0.1		// microphones position
-#define Xr	0.1
+//#define Xr	0.1
 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 */
+    }
     buffer_size = size;
     printf("Bufffer size set to:  %d  Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);
-    /// set the period time
+    // set the period time
     err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);
     if (err < 0)
+    {
         printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));
         return err;
+    }
     return 0;
+}
 ////// SIGNAL GENERATION STUFF
-unsigned int linear_windowed_chirp(short *pole)
+unsigned int linear_windowed_chirp(short *pole)  // generate the ping signal
+{
     unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
     static const float f0 = 5000;		//starting frequency
     static const float fmax = 10000;		//ending frequency
-    static const float Tw = 0.0015;
+    static const float Tw = 0.0015;	// time width of ping in seconds
     static float k;
     unsigned int n=0;
     double t;
     unsigned int chirp_samples;		// number of samples per period
     k=2*(fmax-f0)/Tw;
-    chirp_samples = ceil(rate*Tw);
+    chirp_samples = ceil(rate*Tw);	// compute size of ping sinal in samples
     for (n=0;n<=chirp_samples;n++)
+    {
         t = (double) n / (double)rate;
-        pole[n] = (short) floor( (0.35875 - 0.48829*cos(2*M_PI*t*1/Tw) + 0.14128*cos(2*M_PI*2*t*1/Tw) - 0.01168*cos(2*M_PI*3*t*1/Tw))*maxval*sin(2*M_PI*(t)*(f0+(k/2)*(t))) );
+        pole[n] = (short) floor( (0.35875 - 0.48829*cos(2*M_PI*t*1/Tw) + 0.14128*cos(2*M_PI*2*t*1/Tw) - 0.01168*cos(2*M_PI*3*t*1/Tw))*maxval*sin(2*M_PI*(t)*(f0+(k/2)*(t))) ); // signal generation formula
+    }
-    return (chirp_samples);
+    return (chirp_samples);	// return count of samples in ping
+}
 int main(int argc, char *argv[])
+{
     snd_pcm_t *playback_handle, *capture_handle;
     long int *correlationl, *correlationr;
     float *echo_map;
     int *L_signal, *R_signal;
     short *chirp, *signal;
     float *chirp_spect, *lecho_spect, *recho_spect;
-    float a,b;
+    float a,b, Xl, Xr;
     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
     double k; // sample numbers to distance normalising constant
     unsigned int frequency_bins; // number of output frequency bins
-    double *inchirp;
+    double *inchirp;		// Fourier transform variables
     fftw_complex *outchirp;
     fftw_plan fft_plan_chirp;
-    FILE *out;
+    FILE *out;		// dummy variable for file data output
-    snd_pcm_hw_params_alloca(&hwparams);
+    snd_pcm_hw_params_alloca(&hwparams);	// allocation of soundcard parameters registers
     snd_pcm_sw_params_alloca(&swparams);
     printf("Simple PC sonar $Rev:$ starting work.. \n");
 //open and set playback device
     chirp = calloc(2*period_size, sizeof(short));
     signal = malloc(2*period_size * sizeof(short));
     echo_map = malloc(3*period_size*period_size * sizeof(float));   // Array to store two dimensional image of echos
     if (echo_map == NULL) printf("Can't allocate enought memory");
-    k = SOUND_SPEED/rate; // normalising constant
+    k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance
 // generate ping pattern
     chirp_size = linear_windowed_chirp(chirp);
     frequency_bins = chirp_size / 2 + 1;
             r += chirp[m]*R_signal[m+n];	// correlate with right channel
+        }
         correlationl[n]=abs(l);
         correlationr[n]=abs(r);
+    }
+Xl=-0.1;
+Xr=0.1;
     m=0;
     printf("Building echo map\n");		// compute map from left and right correlation data
-	for (i=0;i < period_size; i++)
+	for (i=0;i < 200; i++)
+	{
 		a=k*i;
-		for(j=0;j < period_size; j++)
+		for(j=0;j < 200; j++)
+		{
 			b=k*j;
-			if( (b+a) >= (Xr-Xl))
+			if( (b+a) >= (Xr-Xl) )
+			{
+//a=10;
+//b=0;
-				echo_map[m]=a;  ///(-a*a+b*b+Xl*Xl+Xr*Xr)/(2*Xl-2*Xr);
+				echo_map[m]=-((a*a)-(b*b)+(Xl-Xr)*(Xl-Xr))/(2*(Xl-Xr));
-				echo_map[m+1]=sqrt((a-b-Xl-Xr)*(a+b+Xl+Xr)*(a-b-Xl+Xr)*(a-b-Xl+Xr)*(a+b-Xl+Xr))/(-2*(Xl-Xr)*(Xl-Xr));
+				echo_map[m+1]=sqrt((a-b-Xl-Xr)*(a+b+Xl+Xr)*(a-b-Xl+Xr)*(a-b-Xl+Xr)*(a+b-Xl+Xr)/(-2*(Xl-Xr)*(Xl-Xr)));
 				echo_map[m+2]=correlationl[i]+correlationr[j];
 				m+=3;
+			}
+		}
+	}
+j=0;
+    for (i=300;i < 700; i++) // print some debugg data from echo map
+    {
+	printf("% 4.3f %4.3f %6.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);
+	j+=3;
+    }
     printf("Searching echos\n");
     r=0;
     l=0;
     for (n=0; n < period_size;n++) 			//najde nejvetsi korelace
+    {

Subversion Repositories svnkaklik

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