Subversion Repositories svnkaklik

Rev

Rev 646 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 646 Rev 647
Line 205... Line 205...
205 
    long int *correlationl, *correlationr;
 205 
    long int *correlationl, *correlationr;
206 
    float *echo_map;
 206 
    float *echo_map;
207 
    int *L_signal, *R_signal;
 207 
    int *L_signal, *R_signal;
208 
    short *chirp, *signal;
 208 
    short *chirp, *signal;
209 
    float *chirp_spect, *lecho_spect, *recho_spect;
 209 
    float *chirp_spect, *lecho_spect, *recho_spect;
210 
    float x,y;
 210 
    float a,b;
211 
    unsigned int i,j,m,n;
 211 
    unsigned int i,j,m,n;
212 
    unsigned int delayl[10],delayr[10];	//store delay of signifed correlation
 212 
    unsigned int delayl[10],delayr[10];	//store delay of signifed correlation
213 
    long int l,r;  // store correlation at strict time
 213 
    long int l,r;  // store correlation at strict time
214 
    double df;	//frequency resolution
214 
    double df;	//frequency resolution
- 
 
 215 
    double k; // sample numbers to distance normalising constant
215 
    unsigned int frequency_bins; // number of output frequency bins
216 
    unsigned int frequency_bins; // number of output frequency bins
216 
 
 217 
 
217 
    double *inchirp;
 218 
    double *inchirp;
218 
    fftw_complex *outchirp;
 219 
    fftw_complex *outchirp;
219 
    fftw_plan fft_plan_chirp;
 220 
    fftw_plan fft_plan_chirp;
Line 272... Line 273...
272 
    correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve
 273 
    correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve
273 
    L_signal = malloc(period_size * sizeof(int));
 274 
    L_signal = malloc(period_size * sizeof(int));
274 
    R_signal = malloc(period_size * sizeof(int));
 275 
    R_signal = malloc(period_size * sizeof(int));
275 
    chirp = calloc(2*period_size, sizeof(short));
 276 
    chirp = calloc(2*period_size, sizeof(short));
276 
    signal = malloc(2*period_size * sizeof(short));
 277 
    signal = malloc(2*period_size * sizeof(short));
277 
    echo_map = malloc(3*period_size * sizeof(float));   // Array to store two dimensional image of echos
 278 
    echo_map = malloc(3*period_size*period_size * sizeof(float));   // Array to store two dimensional image of echos
- 
 
 279 
    if (echo_map == NULL) printf("Can't allocate enought memory");
278 
 
 280 
 
- 
 
 281 
    k = SOUND_SPEED/rate;
279 
// generate ping pattern
 282 
// generate ping pattern
280 
    chirp_size = linear_windowed_chirp(chirp);
 283 
    chirp_size = linear_windowed_chirp(chirp);
281 
 
 284 
 
282 
    frequency_bins = chirp_size / 2 + 1;
 285 
    frequency_bins = chirp_size / 2 + 1;
283 
    df = (double) rate / (double) chirp_size;
 286 
    df = (double) rate / (double) chirp_size;
Line 363... Line 366...
363 
        correlationr[n]=abs(r);
 366 
        correlationr[n]=abs(r);
364 
    }
 367 
    }
365 
 
 368 
 
366 
    m=0;
 369 
    m=0;
367 
    printf("Building echo map\n");		// compute map from left and right correlation data
 370 
    printf("Building echo map\n");		// compute map from left and right correlation data
368 
	for (i=0;i < period_size; i++)
 371 
	for (i=0;i < period_size; i+=10)
369 
	{
 372 
	{
- 
 
 373 
		a=k*i;
370 
		for(j=0;j < period_size; j++)
 374 
		for(j=0;j < period_size; j+=10)
371 
		{
 375 
		{
- 
 
 376 
			b=k*j;
372 
			echo_map[m]=(i*i-j*j+APERTURE*APERTURE)/(2*APERTURE);
 377 
			echo_map[m]=(a*a-b*b+APERTURE*APERTURE)/(2*APERTURE);
373 
			echo_map[m+1]=sqrt(-(i-j-APERTURE)*(i+j-APERTURE)*(i-j+APERTURE)*(i+j+APERTURE))/(2*r);
 378 
			echo_map[m+1]=sqrt((a-b-APERTURE)*(a+b-APERTURE)*(a-b+APERTURE)*(a+b+APERTURE))/(2*r);
374 
			echo_map[m+2]=correlationl[i]*correlationr[j];
 379 
			echo_map[m+2]=correlationl[i]*correlationr[j];
375 
			m+=3;
 380 
			m+=3;
376 
		}
 381 
		}
377 
	}
 382 
	}
378 
 
 - 
 
379 
 
 - 
 
380 
    printf("Searching echos\n");
 383 
    printf("Searching echos\n");
381 
    r=0;
 384 
    r=0;
382 
    l=0;
 385 
    l=0;
383 
    for (n=0; n < period_size;n++) 			//najde nejvetsi korelace
 386 
    for (n=0; n < period_size;n++) 			//najde nejvetsi korelace
384 
    {
 387 
    {
Line 407... Line 410...
407 
 
 410 
 
408 
    printf("Writing output files\n");
 411 
    printf("Writing output files\n");
409 
    out=fopen("/tmp/sonar.txt","w");
 412 
    out=fopen("/tmp/sonar.txt","w");
410 
    for (i=0; i <= (period_size - 1); i++)
 413 
    for (i=0; i <= (period_size - 1); i++)
411 
    {
 414 
    {
412 
        fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",SOUND_SPEED * (float) i / rate,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
 415 
        fprintf(out,"%2.3f %6d %6d %9ld %9ld\n", (float)i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);
413 
    }
 416 
    }
414 
    fclose(out);
 417 
    fclose(out);
415 
 
 418 
 
416 
    j=0;
 419 
    j=0;
417 
    out=fopen("/tmp/plane_cut.txt","w"); // writes plane cut - e.g. density map to file
 420 
    out=fopen("/tmp/plane_cut.txt","w"); // writes plane cut - e.g. density map to file
418 
    for (i=0;i < period_size; i++)
 421 
    for (i=0;i < period_size*period_size; i++)
419 
    {
 422 
    {
420 
	fprintf(out,"%3.2f %3.2f %3.2f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);
 423 
	fprintf(out,"%3.3f %3.3f %3.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);
421 
	j+=3;
 424 
	j+=3;
422 
    }
 425 
    }
- 
 
 426 
    fclose(out);
423 
 
 427 
 
424 
    out=fopen("/tmp/chirp.txt","w");
 428 
    out=fopen("/tmp/chirp.txt","w");
425 
    for (i=0; i <= (chirp_size - 1); i++)
 429 
    for (i=0; i <= (chirp_size - 1); i++)
426 
    {
 430 
    {
427 
        fprintf(out,"%6d %6d\n", i, chirp[i]);
 431 
        fprintf(out,"%6d %6d\n", i, chirp[i]);