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 APERTURE	0.2	// distance between microphones
 
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)
{
    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 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);
 
    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))) );
    }
    return (chirp_samples);
}
 
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;
    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;
    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 $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;
// 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("\nData 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);
    }
 
    m=0;
    printf("Building echo map\n");		// compute map from left and right correlation data
	for (i=0;i < period_size; i+=10)
	{
		a=k*i;
		for(j=0;j < period_size; j+=10)
		{
			b=k*j;
			echo_map[m]=(a*a-b*b+APERTURE*APERTURE)/(2*APERTURE);
			echo_map[m+1]=sqrt((a-b-APERTURE)*(a+b-APERTURE)*(a-b+APERTURE)*(a+b+APERTURE))/(2*r);
			echo_map[m+2]=correlationl[i]*correlationr[j];
			m+=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 plane cut - e.g. density map to file
    for (i=0;i < period_size*period_size; i++)
    {
	fprintf(out,"%3.3f %3.3f %3.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 646	Rev 647
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 APERTURE 0.2 // distance between microphones`	23	`#define APERTURE 0.2 // distance between microphones`
24		24
25	`static char device = "plughw:0,0"; / playback device */`	25	`static char device = "plughw:0,0"; / playback device */`
26	`static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */`	26	`static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */`
27	`static unsigned int rate = 96000; /* stream rate */`	27	`static unsigned int rate = 96000; /* stream rate */`
28	`static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */`	28	`static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */`
29	`static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */`	29	`static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */`
30	`static int resample = 1; /* enable alsa-lib resampling */`	30	`static int resample = 1; /* enable alsa-lib resampling */`
31		31
32	`unsigned int chirp_size;`	32	`unsigned int chirp_size;`
33		33
34	`static snd_pcm_sframes_t buffer_size; // size of buffer at sound card`	34	`static snd_pcm_sframes_t buffer_size; // size of buffer at sound card`
35	`static snd_pcm_sframes_t period_size; //samples per frame`	35	`static snd_pcm_sframes_t period_size; //samples per frame`
36	`static snd_output_t *output = NULL;`	36	`static snd_output_t *output = NULL;`
37		37
38	`static int set_hwparams(snd_pcm_t handle, snd_pcm_hw_params_t params, unsigned int channels)`	38	`static int set_hwparams(snd_pcm_t handle, snd_pcm_hw_params_t params, unsigned int channels)`
39	`{`	39	`{`
40	`unsigned int rrate;`	40	`unsigned int rrate;`
41	`snd_pcm_uframes_t size;`	41	`snd_pcm_uframes_t size;`
42	`int err, dir;`	42	`int err, dir;`
43		43
44	`/* choose all parameters */`	44	`/* choose all parameters */`
45	`err = snd_pcm_hw_params_any(handle, params);`	45	`err = snd_pcm_hw_params_any(handle, params);`
46	`if (err < 0)`	46	`if (err < 0)`
47	`{`	47	`{`
48	`printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));`	48	`printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));`
49	`return err;`	49	`return err;`
50	`}`	50	`}`
51	`/* set hardware resampling */`	51	`/* set hardware resampling */`
52	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`	52	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`
53	`if (err < 0)`	53	`if (err < 0)`
54	`{`	54	`{`
55	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`	55	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`
56	`return err;`	56	`return err;`
57	`}`	57	`}`
58	`/* set the interleaved read/write format */`	58	`/* set the interleaved read/write format */`
59	`err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);`	59	`err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);`
60	`if (err < 0)`	60	`if (err < 0)`
61	`{`	61	`{`
62	`printf("Access type not available for playback: %s\n", snd_strerror(err));`	62	`printf("Access type not available for playback: %s\n", snd_strerror(err));`
63	`return err;`	63	`return err;`
64	`}`	64	`}`
65	`/* set the sample format */`	65	`/* set the sample format */`
66	`err = snd_pcm_hw_params_set_format(handle, params, format);`	66	`err = snd_pcm_hw_params_set_format(handle, params, format);`
67	`if (err < 0)`	67	`if (err < 0)`
68	`{`	68	`{`
69	`printf("Sample format not available for playback: %s\n", snd_strerror(err));`	69	`printf("Sample format not available for playback: %s\n", snd_strerror(err));`
70	`return err;`	70	`return err;`
71	`}`	71	`}`
72	`/* set the count of channels */`	72	`/* set the count of channels */`
73	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`	73	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`
74	`if (err < 0)`	74	`if (err < 0)`
75	`{`	75	`{`
76	`printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));`	76	`printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));`
77	`return err;`	77	`return err;`
78	`}`	78	`}`
79	`/* set the stream rate */`	79	`/* set the stream rate */`
80	`rrate = rate;`	80	`rrate = rate;`
81	`err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);`	81	`err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);`
82	`if (err < 0)`	82	`if (err < 0)`
83	`{`	83	`{`
84	`printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));`	84	`printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));`
85	`return err;`	85	`return err;`
86	`}`	86	`}`
87	`if (rrate != rate)`	87	`if (rrate != rate)`
88	`{`	88	`{`
89	`printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);`	89	`printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);`
90	`return -EINVAL;`	90	`return -EINVAL;`
91	`}`	91	`}`
92	`else printf("Rate set to %i Hz\n", rate, err);`	92	`else printf("Rate set to %i Hz\n", rate, err);`
93	`/* set the buffer time */`	93	`/* set the buffer time */`
94	`err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);`	94	`err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);`
95	`if (err < 0)`	95	`if (err < 0)`
96	`{`	96	`{`
97	`printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));`	97	`printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));`
98	`return err;`	98	`return err;`
99	`}`	99	`}`
100	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`	100	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`
101	`if (err < 0)`	101	`if (err < 0)`
102	`{`	102	`{`
103	`printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));`	103	`printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));`
104	`return err;`	104	`return err;`
105	`}`	105	`}`
106	`buffer_size = size;`	106	`buffer_size = size;`
107	`printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);`	107	`printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);`
108		108
109		109
110	`/// set the period time`	110	`/// set the period time`
111	`err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);`	111	`err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);`
112	`if (err < 0)`	112	`if (err < 0)`
113	`{`	113	`{`
114	`printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));`	114	`printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));`
115	`return err;`	115	`return err;`
116	`}`	116	`}`
117		117
118	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`	118	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`
119	`if (err < 0)`	119	`if (err < 0)`
120	`{`	120	`{`
121	`printf("Unable to get period size for playback: %s\n", snd_strerror(err));`	121	`printf("Unable to get period size for playback: %s\n", snd_strerror(err));`
122	`return err;`	122	`return err;`
123	`}`	123	`}`
124	`period_size = size;`	124	`period_size = size;`
125	`printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);`	125	`printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);`
126		126
127	`/* write the parameters to device */`	127	`/* write the parameters to device */`
128	`err = snd_pcm_hw_params(handle, params);`	128	`err = snd_pcm_hw_params(handle, params);`
129	`if (err < 0)`	129	`if (err < 0)`
130	`{`	130	`{`
131	`printf("Unable to set hw params for playback: %s\n", snd_strerror(err));`	131	`printf("Unable to set hw params for playback: %s\n", snd_strerror(err));`
132	`return err;`	132	`return err;`
133	`}`	133	`}`
134	`return 0;`	134	`return 0;`
135	`}`	135	`}`
136		136
137	`static int set_swparams(snd_pcm_t handle, snd_pcm_sw_params_t swparams)`	137	`static int set_swparams(snd_pcm_t handle, snd_pcm_sw_params_t swparams)`
138	`{`	138	`{`
139	`int err;`	139	`int err;`
140		140
141	`/* get the current swparams */`	141	`/* get the current swparams */`
142	`err = snd_pcm_sw_params_current(handle, swparams);`	142	`err = snd_pcm_sw_params_current(handle, swparams);`
143	`if (err < 0)`	143	`if (err < 0)`
144	`{`	144	`{`
145	`printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));`	145	`printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));`
146	`return err;`	146	`return err;`
147	`}`	147	`}`
148	`// start the transfer when the buffer is almost full: never fou our case`	148	`// start the transfer when the buffer is almost full: never fou our case`
149	`err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);`	149	`err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);`
150	`if (err < 0)`	150	`if (err < 0)`
151	`{`	151	`{`
152	`printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));`	152	`printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));`
153	`return err;`	153	`return err;`
154	`}`	154	`}`
155		155
156	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`	156	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`
157	`if (err < 0)`	157	`if (err < 0)`
158	`{`	158	`{`
159	`printf("Unable to set period event: %s\n", snd_strerror(err));`	159	`printf("Unable to set period event: %s\n", snd_strerror(err));`
160	`return err;`	160	`return err;`
161	`}`	161	`}`
162		162
163	`/* write the parameters to the playback device */`	163	`/* write the parameters to the playback device */`
164	`err = snd_pcm_sw_params(handle, swparams);`	164	`err = snd_pcm_sw_params(handle, swparams);`
165	`if (err < 0)`	165	`if (err < 0)`
166	`{`	166	`{`
167	`printf("Unable to set sw params for playback: %s\n", snd_strerror(err));`	167	`printf("Unable to set sw params for playback: %s\n", snd_strerror(err));`
168	`return err;`	168	`return err;`
169	`}`	169	`}`
170	`return 0;`	170	`return 0;`
171	`}`	171	`}`
172		172
173	`////// SIGNAL GENERATION STUFF`	173	`////// SIGNAL GENERATION STUFF`
174	`unsigned int linear_windowed_chirp(short *pole)`	174	`unsigned int linear_windowed_chirp(short *pole)`
175	`{`	175	`{`
176	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`	176	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`
177		177
178	`static const float f0 = 5000; //starting frequency`	178	`static const float f0 = 5000; //starting frequency`
179	`static const float fmax = 10000; //ending frequency`	179	`static const float fmax = 10000; //ending frequency`
180	`static const float Tw = 0.0015;`	180	`static const float Tw = 0.0015;`
181	`static float k;`	181	`static float k;`
182		182
183	`unsigned int n=0;`	183	`unsigned int n=0;`
184	`double t;`	184	`double t;`
185	`unsigned int chirp_samples; // number of samples per period`	185	`unsigned int chirp_samples; // number of samples per period`
186		186
187	`k=2*(fmax-f0)/Tw;`	187	`k=2*(fmax-f0)/Tw;`
188	`chirp_samples = ceil(rate*Tw);`	188	`chirp_samples = ceil(rate*Tw);`
189		189
190	`for (n=0;n<=chirp_samples;n++)`	190	`for (n=0;n<=chirp_samples;n++)`
191	`{`	191	`{`
192	`t = (double) n / (double)rate;`	192	`t = (double) n / (double)rate;`
193	`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))) );`	193	`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	`}`	194	`}`
195	`return (chirp_samples);`	195	`return (chirp_samples);`
196	`}`	196	`}`
197		197
198	`int main(int argc, char *argv[])`	198	`int main(int argc, char *argv[])`
199	`{`	199	`{`
200	`snd_pcm_t playback_handle, capture_handle;`	200	`snd_pcm_t playback_handle, capture_handle;`
201	`int err;`	201	`int err;`
202	`snd_pcm_hw_params_t *hwparams;`	202	`snd_pcm_hw_params_t *hwparams;`
203	`snd_pcm_sw_params_t *swparams;`	203	`snd_pcm_sw_params_t *swparams;`
204		204
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;`
220		221
221	`FILE *out;`	222	`FILE *out;`
222		223
223	`snd_pcm_hw_params_alloca(&hwparams);`	224	`snd_pcm_hw_params_alloca(&hwparams);`
224	`snd_pcm_sw_params_alloca(&swparams);`	225	`snd_pcm_sw_params_alloca(&swparams);`
225		226
226	`printf("Simple PC sonar $Rev:$ starting work.. \n");`	227	`printf("Simple PC sonar $Rev:$ starting work.. \n");`
227		228
228	`//open and set playback device`	229	`//open and set playback device`
229	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`	230	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`
230	`{`	231	`{`
231	`printf("Playback open error: %s\n", snd_strerror(err));`	232	`printf("Playback open error: %s\n", snd_strerror(err));`
232	`return 0;`	233	`return 0;`
233	`}`	234	`}`
234		235
235	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`	236	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`
236	`{`	237	`{`
237	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	238	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
238	`exit(EXIT_FAILURE);`	239	`exit(EXIT_FAILURE);`
239	`}`	240	`}`
240	`if ((err = set_swparams(playback_handle, swparams)) < 0)`	241	`if ((err = set_swparams(playback_handle, swparams)) < 0)`
241	`{`	242	`{`
242	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	243	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
243	`exit(EXIT_FAILURE);`	244	`exit(EXIT_FAILURE);`
244	`}`	245	`}`
245		246
246	`//open and set capture device`	247	`//open and set capture device`
247	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`	248	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`
248	`{`	249	`{`
249	`printf("Playback open error: %s\n", snd_strerror(err));`	250	`printf("Playback open error: %s\n", snd_strerror(err));`
250	`return 0;`	251	`return 0;`
251	`}`	252	`}`
252		253
253	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`	254	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`
254	`{`	255	`{`
255	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	256	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
256	`exit(EXIT_FAILURE);`	257	`exit(EXIT_FAILURE);`
257	`}`	258	`}`
258	`if ((err = set_swparams(capture_handle, swparams)) < 0)`	259	`if ((err = set_swparams(capture_handle, swparams)) < 0)`
259	`{`	260	`{`
260	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	261	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
261	`exit(EXIT_FAILURE);`	262	`exit(EXIT_FAILURE);`
262	`}`	263	`}`
263		264
264	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together`	265	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together`
265	`if (err < 0)`	266	`if (err < 0)`
266	`{`	267	`{`
267	`printf("Device linking error: %s\n", snd_strerror(err));`	268	`printf("Device linking error: %s\n", snd_strerror(err));`
268	`exit(EXIT_FAILURE);`	269	`exit(EXIT_FAILURE);`
269	`}*/`	270	`}*/`
270		271
271	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`	272	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`
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(2period_size sizeof(short));`	277	`signal = malloc(2period_size sizeof(short));`
277	`echo_map = malloc(3period_size sizeof(float)); // Array to store two dimensional image of echos`	278	`echo_map = malloc(3period_sizeperiod_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;`
284	`chirp_spect = malloc(frequency_bins * sizeof(float));`	287	`chirp_spect = malloc(frequency_bins * sizeof(float));`
285	`lecho_spect = malloc(frequency_bins * sizeof(float));`	288	`lecho_spect = malloc(frequency_bins * sizeof(float));`
286	`recho_spect = malloc(frequency_bins * sizeof(float));`	289	`recho_spect = malloc(frequency_bins * sizeof(float));`
287		290
288	`inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT`	291	`inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT`
289	`outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);`	292	`outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);`
290		293
291	`fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);`	294	`fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);`
292		295
293	`printf("compute chirp spectrum\n");`	296	`printf("compute chirp spectrum\n");`
294	`for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];`	297	`for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];`
295	`fftw_execute(fft_plan_chirp);`	298	`fftw_execute(fft_plan_chirp);`
296	`for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );`	299	`for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );`
297		300
298	`// write chirp data to souncard buffer`	301	`// write chirp data to souncard buffer`
299	`err = snd_pcm_writei(playback_handle, chirp, period_size);`	302	`err = snd_pcm_writei(playback_handle, chirp, period_size);`
300	`if (err < 0)`	303	`if (err < 0)`
301	`{`	304	`{`
302	`printf("Initial write error: %s\n", snd_strerror(err));`	305	`printf("Initial write error: %s\n", snd_strerror(err));`
303	`exit(EXIT_FAILURE);`	306	`exit(EXIT_FAILURE);`
304	`}`	307	`}`
305		308
306	`//start sream`	309	`//start sream`
307	`err = snd_pcm_start(playback_handle);`	310	`err = snd_pcm_start(playback_handle);`
308	`if (err < 0)`	311	`if (err < 0)`
309	`{`	312	`{`
310	`printf("Start error: %s\n", snd_strerror(err));`	313	`printf("Start error: %s\n", snd_strerror(err));`
311	`exit(EXIT_FAILURE);`	314	`exit(EXIT_FAILURE);`
312	`}`	315	`}`
313		316
314	`err = snd_pcm_start(capture_handle);`	317	`err = snd_pcm_start(capture_handle);`
315	`if (err < 0)`	318	`if (err < 0)`
316	`{`	319	`{`
317	`printf("Start error: %s\n", snd_strerror(err));`	320	`printf("Start error: %s\n", snd_strerror(err));`
318	`exit(EXIT_FAILURE);`	321	`exit(EXIT_FAILURE);`
319	`}`	322	`}`
320	`else printf("Transmitting all samples of chirp\n");`	323	`else printf("Transmitting all samples of chirp\n");`
321	`//--------------`	324	`//--------------`
322		325
323	`while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data`	326	`while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data`
324	`{`	327	`{`
325	`usleep(1000);`	328	`usleep(1000);`
326	`printf(".");`	329	`printf(".");`
327	`}`	330	`}`
328		331
329	`err = snd_pcm_drop(playback_handle); // stop audio stream`	332	`err = snd_pcm_drop(playback_handle); // stop audio stream`
330	`err = snd_pcm_drain(capture_handle);`	333	`err = snd_pcm_drain(capture_handle);`
331	`if (err < 0)`	334	`if (err < 0)`
332	`{`	335	`{`
333	`printf("Stop error: %s\n", snd_strerror(err));`	336	`printf("Stop error: %s\n", snd_strerror(err));`
334	`exit(EXIT_FAILURE);`	337	`exit(EXIT_FAILURE);`
335	`}`	338	`}`
336		339
337	`err = snd_pcm_readi(capture_handle, signal, period_size); //read period from audio buffer`	340	`err = snd_pcm_readi(capture_handle, signal, period_size); //read period from audio buffer`
338	`if (err < 0)`	341	`if (err < 0)`
339	`{`	342	`{`
340	`printf("Read error: %s\n", snd_strerror(err));`	343	`printf("Read error: %s\n", snd_strerror(err));`
341	`exit(EXIT_FAILURE);`	344	`exit(EXIT_FAILURE);`
342	`}`	345	`}`
343		346
344	`j=0;`	347	`j=0;`
345	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`	348	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`
346	`{`	349	`{`
347	`L_signal[i]=signal[j];`	350	`L_signal[i]=signal[j];`
348	`R_signal[i]=signal[j+1];`	351	`R_signal[i]=signal[j+1];`
349	`j+=2;`	352	`j+=2;`
350	`}`	353	`}`
351		354
352	`printf("\nData transmitted \ncorrelating\n");`	355	`printf("\nData transmitted \ncorrelating\n");`
353	`for (n=0; n < (period_size - chirp_size - 1); n++)`	356	`for (n=0; n < (period_size - chirp_size - 1); n++)`
354	`{`	357	`{`
355	`l=0;`	358	`l=0;`
356	`r=0;`	359	`r=0;`
357	`for ( m = 0; m < chirp_size;m++)`	360	`for ( m = 0; m < chirp_size;m++)`
358	`{`	361	`{`
359	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`	362	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`
360	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`	363	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`
361	`}`	364	`}`
362	`correlationl[n]=abs(l);`	365	`correlationl[n]=abs(l);`
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]=(ii-jj+APERTUREAPERTURE)/(2APERTURE);`	377	`echo_map[m]=(aa-bb+APERTUREAPERTURE)/(2APERTURE);`
373	`echo_map[m+1]=sqrt(-(i-j-APERTURE)(i+j-APERTURE)(i-j+APERTURE)(i+j+APERTURE))/(2r);`	378	`echo_map[m+1]=sqrt((a-b-APERTURE)(a+b-APERTURE)(a-b+APERTURE)(a+b+APERTURE))/(2r);`
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	`{`
385	`if (l < correlationl[n])`	388	`if (l < correlationl[n])`
386	`{`	389	`{`
387	`delayl[1] = n;`	390	`delayl[1] = n;`
388	`l = correlationl[n];`	391	`l = correlationl[n];`
389	`}`	392	`}`
390	`if (r < correlationr[n])`	393	`if (r < correlationr[n])`
391	`{`	394	`{`
392	`delayr[1] = n;`	395	`delayr[1] = n;`
393	`r = correlationr[n];`	396	`r = correlationr[n];`
394	`}`	397	`}`
395	`}`	398	`}`
396		399
397	`//spocitej frekvencni spektrum pro levy kanal`	400	`//spocitej frekvencni spektrum pro levy kanal`
398	`for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];`	401	`for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];`
399	`fftw_execute(fft_plan_chirp);`	402	`fftw_execute(fft_plan_chirp);`
400	`for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`	403	`for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`
401		404
402		405
403	`// napln pole daty z praveho kanalu a spocitej frekvencni spektrum`	406	`// napln pole daty z praveho kanalu a spocitej frekvencni spektrum`
404	`for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];`	407	`for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];`
405	`fftw_execute(fft_plan_chirp);`	408	`fftw_execute(fft_plan_chirp);`
406	`for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`	409	`for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);`
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]);`
428	`}`	432	`}`
429	`fclose(out);`	433	`fclose(out);`
430		434
431	`out=fopen("/tmp/echo.txt","w");`	435	`out=fopen("/tmp/echo.txt","w");`
432	`for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);`	436	`for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);`
433	`fclose(out);`	437	`fclose(out);`
434		438
435	`out=fopen("/tmp/spektra.txt","w");`	439	`out=fopen("/tmp/spektra.txt","w");`
436	`for (i=0; i < frequency_bins; i++)`	440	`for (i=0; i < frequency_bins; i++)`
437	`{`	441	`{`
438	`fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);`	442	`fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);`
439	`}`	443	`}`
440	`fclose(out);`	444	`fclose(out);`
441		445
442	`free(correlationl);`	446	`free(correlationl);`
443	`free(correlationr);`	447	`free(correlationr);`
444	`free(L_signal);`	448	`free(L_signal);`
445	`free(R_signal);`	449	`free(R_signal);`
446	`free(chirp);`	450	`free(chirp);`
447	`free(signal);`	451	`free(signal);`
448	`free(echo_map);`	452	`free(echo_map);`
449		453
450	`snd_pcm_close(playback_handle);`	454	`snd_pcm_close(playback_handle);`
451	`snd_pcm_close(capture_handle);`	455	`snd_pcm_close(capture_handle);`
452	`return 0;`	456	`return 0;`
453	`}`	457	`}`
454		458

Subversion Repositories svnkaklik

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