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 <sched.h>
#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
//#define Xl	-0.1		// microphones position
//#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 */
static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6;		/* period time in us */
static int resample = 1;				/* enable alsa-lib resampling */
 
unsigned int chirp_size;
 
static snd_pcm_sframes_t buffer_size;	// size of buffer at sound card
static snd_pcm_sframes_t period_size;	//samples per frame
static snd_output_t *output = NULL;
 
static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels)
{
    unsigned int rrate;
    snd_pcm_uframes_t size;
    int err, dir;
 
    /* choose all parameters */
    err = snd_pcm_hw_params_any(handle, params);
    if (err < 0)
    {
        printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));
        return err;
    }
    /* set hardware resampling */
    err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);
    if (err < 0)
    {
        printf("Resampling setup failed for playback: %s\n", snd_strerror(err));
        return err;
    }
    /* set the interleaved read/write format */
    err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);
    if (err < 0)
    {
        printf("Access type not available for playback: %s\n", snd_strerror(err));
        return err;
    }
    /* set the sample format */
    err = snd_pcm_hw_params_set_format(handle, params, format);
    if (err < 0)
    {
        printf("Sample format not available for playback: %s\n", snd_strerror(err));
        return err;
    }
    /* set the count of channels */
    err = snd_pcm_hw_params_set_channels(handle, params, channels);
    if (err < 0)
    {
        printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));
        return err;
    }
    /* set the stream rate */
    rrate = rate;
    err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);
    if (err < 0)
    {
        printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));
        return err;
    }
    if (rrate != rate)
    {
        printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);
        return -EINVAL;
    }
    else printf("Rate set to %i Hz\n", rate, err);
    /* set the buffer time */
    err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);
    if (err < 0)
    {
        printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));
        return err;
    }
    err = snd_pcm_hw_params_get_buffer_size(params, &size);
    if (err < 0)
    {
        printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));
        return err;
    }
    buffer_size = size;
    printf("Bufffer size set to:  %d  Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_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;
    }
 
    err = snd_pcm_hw_params_get_period_size(params, &size, &dir);
    if (err < 0)
    {
        printf("Unable to get period size for playback: %s\n", snd_strerror(err));
        return err;
    }
    period_size = size;
    printf("Period size set to:  %d Requested period time: %ld \n", (int) period_size, (long) period_time);
 
    /* write the parameters to device */
    err = snd_pcm_hw_params(handle, params);
    if (err < 0)
    {
        printf("Unable to set hw params for playback: %s\n", snd_strerror(err));
        return err;
    }
    return 0;
}
 
static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams)
{
    int err;
 
    /* get the current swparams */
    err = snd_pcm_sw_params_current(handle, swparams);
    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;
    }
 
    err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);
    if (err < 0)
    {
        printf("Unable to set period event: %s\n", snd_strerror(err));
        return err;
    }
 
    /* write the parameters to the playback device */
    err = snd_pcm_sw_params(handle, swparams);
    if (err < 0)
    {
        printf("Unable to set sw params for playback: %s\n", snd_strerror(err));
        return err;
    }
    return 0;
}
 
////// SIGNAL GENERATION STUFF
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;	// 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);	// 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))) ); // signal generation formula
    }
    return (chirp_samples);	// return count of samples in ping
}
 
int main(int argc, char *argv[])
{
    snd_pcm_t *playback_handle, *capture_handle;
    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 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;		// Fourier transform variables
    fftw_complex *outchirp;
    fftw_plan fft_plan_chirp;
 
    FILE *out;		// dummy variable for file data output
 
    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
    if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)
    {
        printf("Playback open error: %s\n", snd_strerror(err));
        return 0;
    }
 
    if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)
    {
        printf("Setting of hwparams failed: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
    if ((err = set_swparams(playback_handle, swparams)) < 0)
    {
        printf("Setting of swparams failed: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
 
//open and set capture device
    if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)
    {
        printf("Playback open error: %s\n", snd_strerror(err));
        return 0;
    }
 
    if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)
    {
        printf("Setting of hwparams failed: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
    if ((err = set_swparams(capture_handle, swparams)) < 0)
    {
        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
        if (err < 0)
        {
            printf("Device linking error: %s\n", snd_strerror(err));
            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));
    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 - normalise sample number to distance
 
// 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));
    lecho_spect = malloc(frequency_bins * sizeof(float));
    recho_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);
    }
 
//start sream
    err = snd_pcm_start(playback_handle);
    if (err < 0)
    {
        printf("Start error: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
 
    err = snd_pcm_start(capture_handle);
    if (err < 0)
    {
        printf("Start error: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
    else printf("Transmitting all samples of chirp\n");
//--------------
 
    while ( snd_pcm_avail_update(capture_handle) < period_size)			// wait for one period of data
    {
        usleep(1000);
        printf(".");
    }
 
    err = snd_pcm_drop(playback_handle);		// stop audio stream
    err = snd_pcm_drain(capture_handle);
    if (err < 0)
    {
        printf("Stop error: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
 
    err = snd_pcm_readi(capture_handle, signal, period_size);		//read period from audio buffer
    if (err < 0)
    {
        printf("Read error: %s\n", snd_strerror(err));
        exit(EXIT_FAILURE);
    }
 
    j=0;
    for (i=0;i < period_size;i++)		// separe inretleaved samples to two arrays
    {
        L_signal[i]=signal[j];
        R_signal[i]=signal[j+1];
        j+=2;
    }
 
    printf("\nChirp transmitted \ncorrelating\n");
    for (n=0; n < (period_size - chirp_size - 1); n++)
    {
        l=0;
        r=0;
        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]=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 < 200; i++)
	{
		a=k*i;
		for(j=0;j < 200; j++)
		{
			b=k*j;
			if( (b+a) >= (Xr-Xl) )
			{
//a=10;
//b=0;
				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+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
    {
        if (l < correlationl[n])
        {
            delayl[1] = n;
            l = correlationl[n];
        }
        if (r < correlationr[n])
        {
            delayr[1] = n;
            r = correlationr[n];
        }
    }
 
//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]);
 
 
// napln pole daty z praveho kanalu a spocitej frekvencni spektrum
    for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];
    fftw_execute(fft_plan_chirp);
    for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
 
    printf("Writing output files\n");
    out=fopen("/tmp/sonar.txt","w");
    for (i=0; i <= (period_size - 1); i++)
    {
        fprintf(out,"%2.3f %6d %6d %9ld %9ld\n", (float)i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);
    }
    fclose(out);
 
    j=0;
    out=fopen("/tmp/plane_cut.txt","w"); // writes echo_map - e.g. density map to file
    for (i=0;i < period_size*period_size/100; i++)
    {
	fprintf(out,"% 4.3f %4.3f %6.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);
	j+=3;
    }
    fclose(out);
 
    out=fopen("/tmp/chirp.txt","w");
    for (i=0; i <= (chirp_size - 1); i++)
    {
        fprintf(out,"%6d %6d\n", i, chirp[i]);
    }
    fclose(out);
 
    out=fopen("/tmp/echo.txt","w");
    for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);
    fclose(out);
 
    out=fopen("/tmp/spektra.txt","w");
    for (i=0; i < frequency_bins; 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);
 
    free(correlationl);
    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;
}
 

Rev 651	Rev 652
1	`///////////////////////////////////////////////////////////////////////////////////`	1	`///////////////////////////////////////////////////////////////////////////////////`
2	`// A small demo of sonar.`	2	`// A small demo of sonar.`
3	`// Program allow distance measuring.`	3	`// Program allow distance measuring.`
4	`// Uses cross-correlation algorithm to find echos`	4	`// Uses cross-correlation algorithm to find echos`
5	`//`	5	`//`
6	`// Author: kaklik (kaklik@mlab.cz)`	6	`// Author: kaklik (kaklik@mlab.cz)`
7	`//$Id:$`	7	`//$Id:$`
8	`///////////////////////////////////////////////////////////////////////////////////`	8	`///////////////////////////////////////////////////////////////////////////////////`
9		9
10	`#include <stdio.h>`	10	`#include <stdio.h>`
11	`#include <stdlib.h>`	11	`#include <stdlib.h>`
12	`#include <string.h>`	12	`#include <string.h>`
13	`#include <sched.h>`	13	`#include <sched.h>`
14	`#include <errno.h>`	14	`#include <errno.h>`
15	`#include <getopt.h>`	15	`#include <getopt.h>`
16	`#include <alsa/asoundlib.h>`	16	`#include <alsa/asoundlib.h>`
17	`#include <sys/time.h>`	17	`#include <sys/time.h>`
18	`#include <math.h>`	18	`#include <math.h>`
19	`#include <fftw3.h>`	19	`#include <fftw3.h>`
20		20
21	`#define SOUND_SPEED 340.0 // sound speed in air in metrs per second`	21	`#define SOUND_SPEED 340.0 // sound speed in air in metrs per second`
22	`#define MAX_RANGE 10.0 // maximal working radius in meters`	22	`#define MAX_RANGE 10.0 // maximal working radius in meters`
23	`#define Xl -0.1 // microphones position`	23	`//#define Xl -0.1 // microphones position`
24	`#define Xr 0.1`	24	`//#define Xr 0.1`
25		25
26	`static char device = "plughw:0,0"; / playback device */`	26	`static char device = "plughw:0,0"; / playback device */`
27	`static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */`	27	`static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */`
28	`static unsigned int rate = 96000; /* stream rate */`	28	`static unsigned int rate = 96000; /* stream rate */`
29	`static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */`	29	`static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */`
30	`static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */`	30	`static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */`
31	`static int resample = 1; /* enable alsa-lib resampling */`	31	`static int resample = 1; /* enable alsa-lib resampling */`
32		32
33	`unsigned int chirp_size;`	33	`unsigned int chirp_size;`
34		34
35	`static snd_pcm_sframes_t buffer_size; // size of buffer at sound card`	35	`static snd_pcm_sframes_t buffer_size; // size of buffer at sound card`
36	`static snd_pcm_sframes_t period_size; //samples per frame`	36	`static snd_pcm_sframes_t period_size; //samples per frame`
37	`static snd_output_t *output = NULL;`	37	`static snd_output_t *output = NULL;`
38		38
39	`static int set_hwparams(snd_pcm_t handle, snd_pcm_hw_params_t params, unsigned int channels)`	39	`static int set_hwparams(snd_pcm_t handle, snd_pcm_hw_params_t params, unsigned int channels)`
40	`{`	40	`{`
41	`unsigned int rrate;`	41	`unsigned int rrate;`
42	`snd_pcm_uframes_t size;`	42	`snd_pcm_uframes_t size;`
43	`int err, dir;`	43	`int err, dir;`
44		44
45	`/* choose all parameters */`	45	`/* choose all parameters */`
46	`err = snd_pcm_hw_params_any(handle, params);`	46	`err = snd_pcm_hw_params_any(handle, params);`
47	`if (err < 0)`	47	`if (err < 0)`
48	`{`	48	`{`
49	`printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));`	49	`printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));`
50	`return err;`	50	`return err;`
51	`}`	51	`}`
52	`/* set hardware resampling */`	52	`/* set hardware resampling */`
53	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`	53	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`
54	`if (err < 0)`	54	`if (err < 0)`
55	`{`	55	`{`
56	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`	56	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`
57	`return err;`	57	`return err;`
58	`}`	58	`}`
59	`/* set the interleaved read/write format */`	59	`/* set the interleaved read/write format */`
60	`err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);`	60	`err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);`
61	`if (err < 0)`	61	`if (err < 0)`
62	`{`	62	`{`
63	`printf("Access type not available for playback: %s\n", snd_strerror(err));`	63	`printf("Access type not available for playback: %s\n", snd_strerror(err));`
64	`return err;`	64	`return err;`
65	`}`	65	`}`
66	`/* set the sample format */`	66	`/* set the sample format */`
67	`err = snd_pcm_hw_params_set_format(handle, params, format);`	67	`err = snd_pcm_hw_params_set_format(handle, params, format);`
68	`if (err < 0)`	68	`if (err < 0)`
69	`{`	69	`{`
70	`printf("Sample format not available for playback: %s\n", snd_strerror(err));`	70	`printf("Sample format not available for playback: %s\n", snd_strerror(err));`
71	`return err;`	71	`return err;`
72	`}`	72	`}`
73	`/* set the count of channels */`	73	`/* set the count of channels */`
74	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`	74	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`
75	`if (err < 0)`	75	`if (err < 0)`
76	`{`	76	`{`
77	`printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));`	77	`printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));`
78	`return err;`	78	`return err;`
79	`}`	79	`}`
80	`/* set the stream rate */`	80	`/* set the stream rate */`
81	`rrate = rate;`	81	`rrate = rate;`
82	`err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);`	82	`err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);`
83	`if (err < 0)`	83	`if (err < 0)`
84	`{`	84	`{`
85	`printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));`	85	`printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));`
86	`return err;`	86	`return err;`
87	`}`	87	`}`
88	`if (rrate != rate)`	88	`if (rrate != rate)`
89	`{`	89	`{`
90	`printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);`	90	`printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);`
91	`return -EINVAL;`	91	`return -EINVAL;`
92	`}`	92	`}`
93	`else printf("Rate set to %i Hz\n", rate, err);`	93	`else printf("Rate set to %i Hz\n", rate, err);`
94	`/* set the buffer time */`	94	`/* set the buffer time */`
95	`err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);`	95	`err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);`
96	`if (err < 0)`	96	`if (err < 0)`
97	`{`	97	`{`
98	`printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));`	98	`printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));`
99	`return err;`	99	`return err;`
100	`}`	100	`}`
101	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`	101	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`
102	`if (err < 0)`	102	`if (err < 0)`
103	`{`	103	`{`
104	`printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));`	104	`printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));`
105	`return err;`	105	`return err;`
106	`}`	106	`}`
107	`buffer_size = size;`	107	`buffer_size = size;`
108	`printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);`	108	`printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);`
109		109
110		110
111	`/// set the period time`	111	`// set the period time`
112	`err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);`	112	`err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);`
113	`if (err < 0)`	113	`if (err < 0)`
114	`{`	114	`{`
115	`printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));`	115	`printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));`
116	`return err;`	116	`return err;`
117	`}`	117	`}`
118		118
119	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`	119	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`
120	`if (err < 0)`	120	`if (err < 0)`
121	`{`	121	`{`
122	`printf("Unable to get period size for playback: %s\n", snd_strerror(err));`	122	`printf("Unable to get period size for playback: %s\n", snd_strerror(err));`
123	`return err;`	123	`return err;`
124	`}`	124	`}`
125	`period_size = size;`	125	`period_size = size;`
126	`printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);`	126	`printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);`
127		127
128	`/* write the parameters to device */`	128	`/* write the parameters to device */`
129	`err = snd_pcm_hw_params(handle, params);`	129	`err = snd_pcm_hw_params(handle, params);`
130	`if (err < 0)`	130	`if (err < 0)`
131	`{`	131	`{`
132	`printf("Unable to set hw params for playback: %s\n", snd_strerror(err));`	132	`printf("Unable to set hw params for playback: %s\n", snd_strerror(err));`
133	`return err;`	133	`return err;`
134	`}`	134	`}`
135	`return 0;`	135	`return 0;`
136	`}`	136	`}`
137		137
138	`static int set_swparams(snd_pcm_t handle, snd_pcm_sw_params_t swparams)`	138	`static int set_swparams(snd_pcm_t handle, snd_pcm_sw_params_t swparams)`
139	`{`	139	`{`
140	`int err;`	140	`int err;`
141		141
142	`/* get the current swparams */`	142	`/* get the current swparams */`
143	`err = snd_pcm_sw_params_current(handle, swparams);`	143	`err = snd_pcm_sw_params_current(handle, swparams);`
144	`if (err < 0)`	144	`if (err < 0)`
145	`{`	145	`{`
146	`printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));`	146	`printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));`
147	`return err;`	147	`return err;`
148	`}`	148	`}`
149	`// start the transfer when the buffer is almost full: never fou our case`	149	`// start the transfer when the buffer is almost full: never fou our case`
150	`err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);`	150	`err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);`
151	`if (err < 0)`	151	`if (err < 0)`
152	`{`	152	`{`
153	`printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));`	153	`printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));`
154	`return err;`	154	`return err;`
155	`}`	155	`}`
156		156
157	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`	157	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`
158	`if (err < 0)`	158	`if (err < 0)`
159	`{`	159	`{`
160	`printf("Unable to set period event: %s\n", snd_strerror(err));`	160	`printf("Unable to set period event: %s\n", snd_strerror(err));`
161	`return err;`	161	`return err;`
162	`}`	162	`}`
163		163
164	`/* write the parameters to the playback device */`	164	`/* write the parameters to the playback device */`
165	`err = snd_pcm_sw_params(handle, swparams);`	165	`err = snd_pcm_sw_params(handle, swparams);`
166	`if (err < 0)`	166	`if (err < 0)`
167	`{`	167	`{`
168	`printf("Unable to set sw params for playback: %s\n", snd_strerror(err));`	168	`printf("Unable to set sw params for playback: %s\n", snd_strerror(err));`
169	`return err;`	169	`return err;`
170	`}`	170	`}`
171	`return 0;`	171	`return 0;`
172	`}`	172	`}`
173		173
174	`////// SIGNAL GENERATION STUFF`	174	`////// SIGNAL GENERATION STUFF`
175	`unsigned int linear_windowed_chirp(short *pole)`	175	`unsigned int linear_windowed_chirp(short *pole) // generate the ping signal`
176	`{`	176	`{`
177	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`	177	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`
178		178
179	`static const float f0 = 5000; //starting frequency`	179	`static const float f0 = 5000; //starting frequency`
180	`static const float fmax = 10000; //ending frequency`	180	`static const float fmax = 10000; //ending frequency`
181	`static const float Tw = 0.0015;`	181	`static const float Tw = 0.0015; // time width of ping in seconds`
182	`static float k;`	182	`static float k;`
183		183
184	`unsigned int n=0;`	184	`unsigned int n=0;`
185	`double t;`	185	`double t;`
186	`unsigned int chirp_samples; // number of samples per period`	186	`unsigned int chirp_samples; // number of samples per period`
187		187
188	`k=2*(fmax-f0)/Tw;`	188	`k=2*(fmax-f0)/Tw;`
189	`chirp_samples = ceil(rate*Tw);`	189	`chirp_samples = ceil(rate*Tw); // compute size of ping sinal in samples`
190		190
191	`for (n=0;n<=chirp_samples;n++)`	191	`for (n=0;n<=chirp_samples;n++)`
192	`{`	192	`{`
193	`t = (double) n / (double)rate;`	193	`t = (double) n / (double)rate;`
194	`pole[n] = (short) floor( (0.35875 - 0.48829cos(2M_PIt1/Tw) + 0.14128cos(2M_PI2t1/Tw) - 0.01168cos(2M_PI3t1/Tw))maxvalsin(2M_PI(t)(f0+(k/2)(t))) );`	194	`pole[n] = (short) floor( (0.35875 - 0.48829cos(2M_PIt1/Tw) + 0.14128cos(2M_PI2t1/Tw) - 0.01168cos(2M_PI3t1/Tw))maxvalsin(2M_PI(t)(f0+(k/2)(t))) ); // signal generation formula`
195	`}`	195	`}`
196	`return (chirp_samples);`	196	`return (chirp_samples); // return count of samples in ping`
197	`}`	197	`}`
198		198
199	`int main(int argc, char *argv[])`	199	`int main(int argc, char *argv[])`
200	`{`	200	`{`
201	`snd_pcm_t playback_handle, capture_handle;`	201	`snd_pcm_t playback_handle, capture_handle;`
202	`int err;`	202	`int err;`
203	`snd_pcm_hw_params_t *hwparams;`	203	`snd_pcm_hw_params_t *hwparams;`
204	`snd_pcm_sw_params_t *swparams;`	204	`snd_pcm_sw_params_t *swparams;`
205		205
206	`long int correlationl, correlationr;`	206	`long int correlationl, correlationr;`
207	`float *echo_map;`	207	`float *echo_map;`
208	`int L_signal, R_signal;`	208	`int L_signal, R_signal;`
209	`short chirp, signal;`	209	`short chirp, signal;`
210	`float chirp_spect, lecho_spect, *recho_spect;`	210	`float chirp_spect, lecho_spect, *recho_spect;`
211	`float a,b;`	211	`float a,b, Xl, Xr;`
212	`unsigned int i,j,m,n;`	212	`unsigned int i,j,m,n;`
213	`unsigned int delayl[10],delayr[10]; //store delay of signifed correlation`	213	`unsigned int delayl[10],delayr[10]; //store delay of signifed correlation`
214	`long int l,r; // store correlation at strict time`	214	`long int l,r; // store correlation at strict time`
215	`double df; //frequency resolution`	215	`double df; //frequency resolution`
216	`double k; // sample numbers to distance normalising constant`	216	`double k; // sample numbers to distance normalising constant`
217	`unsigned int frequency_bins; // number of output frequency bins`	217	`unsigned int frequency_bins; // number of output frequency bins`
218		218
219	`double *inchirp;`	219	`double *inchirp; // Fourier transform variables`
220	`fftw_complex *outchirp;`	220	`fftw_complex *outchirp;`
221	`fftw_plan fft_plan_chirp;`	221	`fftw_plan fft_plan_chirp;`
222		222
223	`FILE *out;`	223	`FILE *out; // dummy variable for file data output`
224		224
225	`snd_pcm_hw_params_alloca(&hwparams);`	225	`snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registers`
226	`snd_pcm_sw_params_alloca(&swparams);`	226	`snd_pcm_sw_params_alloca(&swparams);`
227		227
228	`printf("Simple PC sonar $Rev:$ starting work.. \n");`	228	`printf("Simple PC sonar $Rev:$ starting work.. \n");`
229		229
230	`//open and set playback device`	230	`//open and set playback device`
231	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`	231	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`
232	`{`	232	`{`
233	`printf("Playback open error: %s\n", snd_strerror(err));`	233	`printf("Playback open error: %s\n", snd_strerror(err));`
234	`return 0;`	234	`return 0;`
235	`}`	235	`}`
236		236
237	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`	237	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`
238	`{`	238	`{`
239	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	239	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
240	`exit(EXIT_FAILURE);`	240	`exit(EXIT_FAILURE);`
241	`}`	241	`}`
242	`if ((err = set_swparams(playback_handle, swparams)) < 0)`	242	`if ((err = set_swparams(playback_handle, swparams)) < 0)`
243	`{`	243	`{`
244	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	244	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
245	`exit(EXIT_FAILURE);`	245	`exit(EXIT_FAILURE);`
246	`}`	246	`}`
247		247
248	`//open and set capture device`	248	`//open and set capture device`
249	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`	249	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`
250	`{`	250	`{`
251	`printf("Playback open error: %s\n", snd_strerror(err));`	251	`printf("Playback open error: %s\n", snd_strerror(err));`
252	`return 0;`	252	`return 0;`
253	`}`	253	`}`
254		254
255	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`	255	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`
256	`{`	256	`{`
257	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	257	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
258	`exit(EXIT_FAILURE);`	258	`exit(EXIT_FAILURE);`
259	`}`	259	`}`
260	`if ((err = set_swparams(capture_handle, swparams)) < 0)`	260	`if ((err = set_swparams(capture_handle, swparams)) < 0)`
261	`{`	261	`{`
262	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	262	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
263	`exit(EXIT_FAILURE);`	263	`exit(EXIT_FAILURE);`
264	`}`	264	`}`
265		265
266	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together`	266	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together`
267	`if (err < 0)`	267	`if (err < 0)`
268	`{`	268	`{`
269	`printf("Device linking error: %s\n", snd_strerror(err));`	269	`printf("Device linking error: %s\n", snd_strerror(err));`
270	`exit(EXIT_FAILURE);`	270	`exit(EXIT_FAILURE);`
271	`}*/`	271	`}*/`
272		272
273	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`	273	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`
274	`correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve`	274	`correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve`
275	`L_signal = malloc(period_size * sizeof(int));`	275	`L_signal = malloc(period_size * sizeof(int));`
276	`R_signal = malloc(period_size * sizeof(int));`	276	`R_signal = malloc(period_size * sizeof(int));`
277	`chirp = calloc(2*period_size, sizeof(short));`	277	`chirp = calloc(2*period_size, sizeof(short));`
278	`signal = malloc(2period_size sizeof(short));`	278	`signal = malloc(2period_size sizeof(short));`
279	`echo_map = malloc(3period_sizeperiod_size * sizeof(float)); // Array to store two dimensional image of echos`	279	`echo_map = malloc(3period_sizeperiod_size * sizeof(float)); // Array to store two dimensional image of echos`
280	`if (echo_map == NULL) printf("Can't allocate enought memory");`	280	`if (echo_map == NULL) printf("Can't allocate enought memory");`
281		281
282	`k = SOUND_SPEED/rate; // normalising constant`	282	`k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance`
283		283
284	`// generate ping pattern`	284	`// generate ping pattern`
285	`chirp_size = linear_windowed_chirp(chirp);`	285	`chirp_size = linear_windowed_chirp(chirp);`
286		286
287	`frequency_bins = chirp_size / 2 + 1;`	287	`frequency_bins = chirp_size / 2 + 1;`
288	`df = (double) rate / (double) chirp_size;`	288	`df = (double) rate / (double) chirp_size;`
289	`chirp_spect = malloc(frequency_bins * sizeof(float));`	289	`chirp_spect = malloc(frequency_bins * sizeof(float));`
290	`lecho_spect = malloc(frequency_bins * sizeof(float));`	290	`lecho_spect = malloc(frequency_bins * sizeof(float));`
291	`recho_spect = malloc(frequency_bins * sizeof(float));`	291	`recho_spect = malloc(frequency_bins * sizeof(float));`
292		292
293	`inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT`	293	`inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT`
294	`outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);`	294	`outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);`
295		295
296	`fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);`	296	`fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);`
297		297
298	`printf("compute chirp spectrum\n");`	298	`printf("compute chirp spectrum\n");`
299	`for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];`	299	`for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];`
300	`fftw_execute(fft_plan_chirp);`	300	`fftw_execute(fft_plan_chirp);`
301	`for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );`	301	`for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );`
302		302
303	`// write chirp data to souncard buffer`	303	`// write chirp data to souncard buffer`
304	`err = snd_pcm_writei(playback_handle, chirp, period_size);`	304	`err = snd_pcm_writei(playback_handle, chirp, period_size);`
305	`if (err < 0)`	305	`if (err < 0)`
306	`{`	306	`{`
307	`printf("Initial write error: %s\n", snd_strerror(err));`	307	`printf("Initial write error: %s\n", snd_strerror(err));`
308	`exit(EXIT_FAILURE);`	308	`exit(EXIT_FAILURE);`
309	`}`	309	`}`
310		310
311	`//start sream`	311	`//start sream`
312	`err = snd_pcm_start(playback_handle);`	312	`err = snd_pcm_start(playback_handle);`
313	`if (err < 0)`	313	`if (err < 0)`
314	`{`	314	`{`
315	`printf("Start error: %s\n", snd_strerror(err));`	315	`printf("Start error: %s\n", snd_strerror(err));`
316	`exit(EXIT_FAILURE);`	316	`exit(EXIT_FAILURE);`
317	`}`	317	`}`
318		318
319	`err = snd_pcm_start(capture_handle);`	319	`err = snd_pcm_start(capture_handle);`
320	`if (err < 0)`	320	`if (err < 0)`
321	`{`	321	`{`
322	`printf("Start error: %s\n", snd_strerror(err));`	322	`printf("Start error: %s\n", snd_strerror(err));`
323	`exit(EXIT_FAILURE);`	323	`exit(EXIT_FAILURE);`
324	`}`	324	`}`
325	`else printf("Transmitting all samples of chirp\n");`	325	`else printf("Transmitting all samples of chirp\n");`
326	`//--------------`	326	`//--------------`
327		327
328	`while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data`	328	`while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data`
329	`{`	329	`{`
330	`usleep(1000);`	330	`usleep(1000);`
331	`printf(".");`	331	`printf(".");`
332	`}`	332	`}`
333		333
334	`err = snd_pcm_drop(playback_handle); // stop audio stream`	334	`err = snd_pcm_drop(playback_handle); // stop audio stream`
335	`err = snd_pcm_drain(capture_handle);`	335	`err = snd_pcm_drain(capture_handle);`
336	`if (err < 0)`	336	`if (err < 0)`
337	`{`	337	`{`
338	`printf("Stop error: %s\n", snd_strerror(err));`	338	`printf("Stop error: %s\n", snd_strerror(err));`
339	`exit(EXIT_FAILURE);`	339	`exit(EXIT_FAILURE);`
340	`}`	340	`}`
341		341
342	`err = snd_pcm_readi(capture_handle, signal, period_size); //read period from audio buffer`	342	`err = snd_pcm_readi(capture_handle, signal, period_size); //read period from audio buffer`
343	`if (err < 0)`	343	`if (err < 0)`
344	`{`	344	`{`
345	`printf("Read error: %s\n", snd_strerror(err));`	345	`printf("Read error: %s\n", snd_strerror(err));`
346	`exit(EXIT_FAILURE);`	346	`exit(EXIT_FAILURE);`
347	`}`	347	`}`
348		348
349	`j=0;`	349	`j=0;`
350	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`	350	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`
351	`{`	351	`{`
352	`L_signal[i]=signal[j];`	352	`L_signal[i]=signal[j];`
353	`R_signal[i]=signal[j+1];`	353	`R_signal[i]=signal[j+1];`
354	`j+=2;`	354	`j+=2;`
355	`}`	355	`}`
356		356
357	`printf("\nChirp transmitted \ncorrelating\n");`	357	`printf("\nChirp transmitted \ncorrelating\n");`
358	`for (n=0; n < (period_size - chirp_size - 1); n++)`	358	`for (n=0; n < (period_size - chirp_size - 1); n++)`
359	`{`	359	`{`
360	`l=0;`	360	`l=0;`
361	`r=0;`	361	`r=0;`
362	`for ( m = 0; m < chirp_size;m++)`	362	`for ( m = 0; m < chirp_size;m++)`
363	`{`	363	`{`
364	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`	364	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`
365	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`	365	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`
366	`}`	366	`}`
367	`correlationl[n]=abs(l);`	367	`correlationl[n]=abs(l);`
368	`correlationr[n]=abs(r);`	368	`correlationr[n]=abs(r);`
369	`}`	369	`}`
-		370	`Xl=-0.1;`
370		371	`Xr=0.1;`
371	`m=0;`	372	`m=0;`
372	`printf("Building echo map\n"); // compute map from left and right correlation data`	373	`printf("Building echo map\n"); // compute map from left and right correlation data`
373	`for (i=0;i < period_size; i++)`	374	`for (i=0;i < 200; i++)`
374	`{`	375	`{`
375	`a=k*i;`	376	`a=k*i;`
376	`for(j=0;j < period_size; j++)`	377	`for(j=0;j < 200; j++)`
377	`{`	378	`{`
378	`b=k*j;`	379	`b=k*j;`
379	`if( (b+a) >= (Xr-Xl))`	380	`if( (b+a) >= (Xr-Xl) )`
380	`{`	381	`{`
-		382	`//a=10;`
-		383	`//b=0;`
381	`echo_map[m]=a; ///(-aa+bb+XlXl+XrXr)/(2Xl-2Xr);`	384	`echo_map[m]=-((aa)-(bb)+(Xl-Xr)(Xl-Xr))/(2(Xl-Xr));`
382	`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));`	385	`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)));`
383	`echo_map[m+2]=correlationl[i]+correlationr[j];`	386	`echo_map[m+2]=correlationl[i]+correlationr[j];`
384	`m+=3;`	387	`m+=3;`
385	`}`	388	`}`
386	`}`	389	`}`
387	`}`	390	`}`
-		391	`j=0;`
-		392	`for (i=300;i < 700; i++) // print some debugg data from echo map`
-		393	`{`
-		394	`printf("% 4.3f %4.3f %6.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);`
-		395	`j+=3;`
-		396	`}`
-		397
388	`printf("Searching echos\n");`	398	`printf("Searching echos\n");`
389	`r=0;`	399	`r=0;`
390	`l=0;`	400	`l=0;`
391	`for (n=0; n < period_size;n++) //najde nejvetsi korelace`	401	`for (n=0; n < period_size;n++) //najde nejvetsi korelace`
392	`{`	402	`{`
393	`if (l < correlationl[n])`	403	`if (l < correlationl[n])`
394	`{`	404	`{`
395	`delayl[1] = n;`	405	`delayl[1] = n;`
396	`l = correlationl[n];`	406	`l = correlationl[n];`
397	`}`	407	`}`
398	`if (r < correlationr[n])`	408	`if (r < correlationr[n])`
399	`{`	409	`{`
400	`delayr[1] = n;`	410	`delayr[1] = n;`
401	`r = correlationr[n];`	411	`r = correlationr[n];`
402	`}`	412	`}`
403	`}`	413	`}`
404		414
405	`//spocitej frekvencni spektrum pro levy kanal`	415	`//spocitej frekvencni spektrum pro levy kanal`
406	`for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];`	416	`for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];`
407	`fftw_execute(fft_plan_chirp);`	417	`fftw_execute(fft_plan_chirp);`
408	`for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`	418	`for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`
409		419
410		420
411	`// napln pole daty z praveho kanalu a spocitej frekvencni spektrum`	421	`// napln pole daty z praveho kanalu a spocitej frekvencni spektrum`
412	`for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];`	422	`for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];`
413	`fftw_execute(fft_plan_chirp);`	423	`fftw_execute(fft_plan_chirp);`
414	`for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`	424	`for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`
415		425
416	`printf("Writing output files\n");`	426	`printf("Writing output files\n");`
417	`out=fopen("/tmp/sonar.txt","w");`	427	`out=fopen("/tmp/sonar.txt","w");`
418	`for (i=0; i <= (period_size - 1); i++)`	428	`for (i=0; i <= (period_size - 1); i++)`
419	`{`	429	`{`
420	`fprintf(out,"%2.3f %6d %6d %9ld %9ld\n", (float)i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);`	430	`fprintf(out,"%2.3f %6d %6d %9ld %9ld\n", (float)i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);`
421	`}`	431	`}`
422	`fclose(out);`	432	`fclose(out);`
423		433
424	`j=0;`	434	`j=0;`
425	`out=fopen("/tmp/plane_cut.txt","w"); // writes echo_map - e.g. density map to file`	435	`out=fopen("/tmp/plane_cut.txt","w"); // writes echo_map - e.g. density map to file`
426	`for (i=0;i < period_size*period_size/100; i++)`	436	`for (i=0;i < period_size*period_size/100; i++)`
427	`{`	437	`{`
428	`fprintf(out,"% 4.3f %4.3f %6.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);`	438	`fprintf(out,"% 4.3f %4.3f %6.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);`
429	`j+=3;`	439	`j+=3;`
430	`}`	440	`}`
431	`fclose(out);`	441	`fclose(out);`
432		442
433	`out=fopen("/tmp/chirp.txt","w");`	443	`out=fopen("/tmp/chirp.txt","w");`
434	`for (i=0; i <= (chirp_size - 1); i++)`	444	`for (i=0; i <= (chirp_size - 1); i++)`
435	`{`	445	`{`
436	`fprintf(out,"%6d %6d\n", i, chirp[i]);`	446	`fprintf(out,"%6d %6d\n", i, chirp[i]);`
437	`}`	447	`}`
438	`fclose(out);`	448	`fclose(out);`
439		449
440	`out=fopen("/tmp/echo.txt","w");`	450	`out=fopen("/tmp/echo.txt","w");`
441	`for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);`	451	`for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);`
442	`fclose(out);`	452	`fclose(out);`
443		453
444	`out=fopen("/tmp/spektra.txt","w");`	454	`out=fopen("/tmp/spektra.txt","w");`
445	`for (i=0; i < frequency_bins; i++)`	455	`for (i=0; i < frequency_bins; i++)`
446	`{`	456	`{`
447	`fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);`	457	`fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);`
448	`}`	458	`}`
449	`fclose(out);`	459	`fclose(out);`
450		460
451	`free(correlationl);`	461	`free(correlationl);`
452	`free(correlationr);`	462	`free(correlationr);`
453	`free(L_signal);`	463	`free(L_signal);`
454	`free(R_signal);`	464	`free(R_signal);`
455	`free(chirp);`	465	`free(chirp);`
456	`free(signal);`	466	`free(signal);`
457	`free(echo_map);`	467	`free(echo_map);`
458		468
459	`snd_pcm_close(playback_handle);`	469	`snd_pcm_close(playback_handle);`
460	`snd_pcm_close(capture_handle);`	470	`snd_pcm_close(capture_handle);`
461	`return 0;`	471	`return 0;`
462	`}`	472	`}`
463		473

Subversion Repositories svnkaklik

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