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)
//
///////////////////////////////////////////////////////////////////////////////////
 
#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
 
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 = 13000;		//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;
    int *L_signal, *R_signal;
    short *chirp, *signal;
    float *chirp_spect, *echo_spect;
    unsigned int i,j,m,n;
    unsigned int delayl[10],delayr[10];	//store delay of signifed correlation
    long int l,r;  // store correlation at strict time
    double df;	//frequency resolution 
    unsigned int frequency_bins; // number of output frequency bins 
 
    double *inchirp;
    fftw_complex *outchirp;
    fftw_plan fft_plan_chirp;
 
    FILE *out;
 
    snd_pcm_hw_params_alloca(&hwparams);
    snd_pcm_sw_params_alloca(&swparams);
 
    printf("Simple PC sonar ver. 000000001 starting work.. \n");
 
//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));
 
// 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));
    echo_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("Waiting for transmitt all samples\n");
//--------------
 
    while ( snd_pcm_avail_update(capture_handle) < period_size)
    {
        usleep(1000);
        printf(".");
    }
 
    err = snd_pcm_drop(playback_handle);
    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);
    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);
    }
 
    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];
        }
    }
 
    
 
    printf("Writing output file\n");
    out=fopen("/tmp/sonar.txt","w");
    j=0;
    for (i=0; i <= (period_size - 1); i++)
    {
        fprintf(out,"%6d %6d %6d %9ld %9ld\n",i,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
        j+=2;
    }
    fclose(out);
 
    out=fopen("/tmp/chirp.txt","w");
    j=0;
    for (i=0; i <= (chirp_size - 1); i++)
    {
        fprintf(out,"%6d %6d %f\n", i, chirp[i], chirp_spect[i]);
        j+=2;
    }
    fclose(out);
 
    printf("Echo zacina na: %d vzorku.\n", delayl[1]);
    printf("Casove na: %f s\n", ((float)delayl[1]/rate));
    printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delayl[1]/rate));
 
    free(correlationl);
    free(correlationr);
    free(L_signal);
    free(R_signal);
    free(chirp);
    free(signal);
 
    snd_pcm_close(playback_handle);
    snd_pcm_close(capture_handle);
    return 0;
}
 

Rev 562	Rev 563
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	`//`	7	`//`
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		20
20	`#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`
21	`#define MAX_RANGE 10.0 // maximal working radius in meters`	22	`#define MAX_RANGE 10.0 // maximal working radius in meters`
22		23
23	`static char device = "plughw:0,0"; / playback device */`	24	`static char device = "plughw:0,0"; / playback device */`
24	`static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */`	25	`static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */`
25	`static unsigned int rate = 96000; /* stream rate */`	26	`static unsigned int rate = 96000; /* stream rate */`
26	`static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */`	27	`static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */`
27	`static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */`	28	`static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */`
28	`static int resample = 1; /* enable alsa-lib resampling */`	29	`static int resample = 1; /* enable alsa-lib resampling */`
29		30
30	`unsigned int chirp_size;`	31	`unsigned int chirp_size;`
31		32
32	`static snd_pcm_sframes_t buffer_size; // size of buffer at sound card`	33	`static snd_pcm_sframes_t buffer_size; // size of buffer at sound card`
33	`static snd_pcm_sframes_t period_size; //samples per frame`	34	`static snd_pcm_sframes_t period_size; //samples per frame`
34	`static snd_output_t *output = NULL;`	35	`static snd_output_t *output = NULL;`
35		36
36	`static int set_hwparams(snd_pcm_t handle, snd_pcm_hw_params_t params, unsigned int channels)`	37	`static int set_hwparams(snd_pcm_t handle, snd_pcm_hw_params_t params, unsigned int channels)`
37	`{`	38	`{`
38	`unsigned int rrate;`	39	`unsigned int rrate;`
39	`snd_pcm_uframes_t size;`	40	`snd_pcm_uframes_t size;`
40	`int err, dir;`	41	`int err, dir;`
41		42
42	`/* choose all parameters */`	43	`/* choose all parameters */`
43	`err = snd_pcm_hw_params_any(handle, params);`	44	`err = snd_pcm_hw_params_any(handle, params);`
44	`if (err < 0)`	45	`if (err < 0)`
45	`{`	46	`{`
46	`printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));`	47	`printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));`
47	`return err;`	48	`return err;`
48	`}`	49	`}`
49	`/* set hardware resampling */`	50	`/* set hardware resampling */`
50	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`	51	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`
51	`if (err < 0)`	52	`if (err < 0)`
52	`{`	53	`{`
53	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`	54	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`
54	`return err;`	55	`return err;`
55	`}`	56	`}`
56	`/* set the interleaved read/write format */`	57	`/* set the interleaved read/write format */`
57	`err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);`	58	`err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);`
58	`if (err < 0)`	59	`if (err < 0)`
59	`{`	60	`{`
60	`printf("Access type not available for playback: %s\n", snd_strerror(err));`	61	`printf("Access type not available for playback: %s\n", snd_strerror(err));`
61	`return err;`	62	`return err;`
62	`}`	63	`}`
63	`/* set the sample format */`	64	`/* set the sample format */`
64	`err = snd_pcm_hw_params_set_format(handle, params, format);`	65	`err = snd_pcm_hw_params_set_format(handle, params, format);`
65	`if (err < 0)`	66	`if (err < 0)`
66	`{`	67	`{`
67	`printf("Sample format not available for playback: %s\n", snd_strerror(err));`	68	`printf("Sample format not available for playback: %s\n", snd_strerror(err));`
68	`return err;`	69	`return err;`
69	`}`	70	`}`
70	`/* set the count of channels */`	71	`/* set the count of channels */`
71	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`	72	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`
72	`if (err < 0)`	73	`if (err < 0)`
73	`{`	74	`{`
74	`printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));`	75	`printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));`
75	`return err;`	76	`return err;`
76	`}`	77	`}`
77	`/* set the stream rate */`	78	`/* set the stream rate */`
78	`rrate = rate;`	79	`rrate = rate;`
79	`err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);`	80	`err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);`
80	`if (err < 0)`	81	`if (err < 0)`
81	`{`	82	`{`
82	`printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));`	83	`printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));`
83	`return err;`	84	`return err;`
84	`}`	85	`}`
85	`if (rrate != rate)`	86	`if (rrate != rate)`
86	`{`	87	`{`
87	`printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);`	88	`printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);`
88	`return -EINVAL;`	89	`return -EINVAL;`
89	`}`	90	`}`
90	`else printf("Rate set to %i Hz\n", rate, err);`	91	`else printf("Rate set to %i Hz\n", rate, err);`
91	`/* set the buffer time */`	92	`/* set the buffer time */`
92	`err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);`	93	`err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);`
93	`if (err < 0)`	94	`if (err < 0)`
94	`{`	95	`{`
95	`printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));`	96	`printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));`
96	`return err;`	97	`return err;`
97	`}`	98	`}`
98	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`	99	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`
99	`if (err < 0)`	100	`if (err < 0)`
100	`{`	101	`{`
101	`printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));`	102	`printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));`
102	`return err;`	103	`return err;`
103	`}`	104	`}`
104	`buffer_size = size;`	105	`buffer_size = size;`
105	`printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);`	106	`printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);`
106		107
107		108
108	`/// set the period time`	109	`/// set the period time`
109	`err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);`	110	`err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);`
110	`if (err < 0)`	111	`if (err < 0)`
111	`{`	112	`{`
112	`printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));`	113	`printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));`
113	`return err;`	114	`return err;`
114	`}`	115	`}`
115		116
116	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`	117	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`
117	`if (err < 0)`	118	`if (err < 0)`
118	`{`	119	`{`
119	`printf("Unable to get period size for playback: %s\n", snd_strerror(err));`	120	`printf("Unable to get period size for playback: %s\n", snd_strerror(err));`
120	`return err;`	121	`return err;`
121	`}`	122	`}`
122	`period_size = size;`	123	`period_size = size;`
123	`printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);`	124	`printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);`
124		125
125	`/* write the parameters to device */`	126	`/* write the parameters to device */`
126	`err = snd_pcm_hw_params(handle, params);`	127	`err = snd_pcm_hw_params(handle, params);`
127	`if (err < 0)`	128	`if (err < 0)`
128	`{`	129	`{`
129	`printf("Unable to set hw params for playback: %s\n", snd_strerror(err));`	130	`printf("Unable to set hw params for playback: %s\n", snd_strerror(err));`
130	`return err;`	131	`return err;`
131	`}`	132	`}`
132	`return 0;`	133	`return 0;`
133	`}`	134	`}`
134		135
135	`static int set_swparams(snd_pcm_t handle, snd_pcm_sw_params_t swparams)`	136	`static int set_swparams(snd_pcm_t handle, snd_pcm_sw_params_t swparams)`
136	`{`	137	`{`
137	`int err;`	138	`int err;`
138		139
139	`/* get the current swparams */`	140	`/* get the current swparams */`
140	`err = snd_pcm_sw_params_current(handle, swparams);`	141	`err = snd_pcm_sw_params_current(handle, swparams);`
141	`if (err < 0)`	142	`if (err < 0)`
142	`{`	143	`{`
143	`printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));`	144	`printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));`
144	`return err;`	145	`return err;`
145	`}`	146	`}`
146	`// start the transfer when the buffer is almost full: never fou our case`	147	`// start the transfer when the buffer is almost full: never fou our case`
147	`err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);`	148	`err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);`
148	`if (err < 0)`	149	`if (err < 0)`
149	`{`	150	`{`
150	`printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));`	151	`printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));`
151	`return err;`	152	`return err;`
152	`}`	153	`}`
153		154
154	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`	155	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`
155	`if (err < 0)`	156	`if (err < 0)`
156	`{`	157	`{`
157	`printf("Unable to set period event: %s\n", snd_strerror(err));`	158	`printf("Unable to set period event: %s\n", snd_strerror(err));`
158	`return err;`	159	`return err;`
159	`}`	160	`}`
160		161
161	`/* write the parameters to the playback device */`	162	`/* write the parameters to the playback device */`
162	`err = snd_pcm_sw_params(handle, swparams);`	163	`err = snd_pcm_sw_params(handle, swparams);`
163	`if (err < 0)`	164	`if (err < 0)`
164	`{`	165	`{`
165	`printf("Unable to set sw params for playback: %s\n", snd_strerror(err));`	166	`printf("Unable to set sw params for playback: %s\n", snd_strerror(err));`
166	`return err;`	167	`return err;`
167	`}`	168	`}`
168	`return 0;`	169	`return 0;`
169	`}`	170	`}`
170		171
171	`////// SIGNAL GENERATION STUFF`	172	`////// SIGNAL GENERATION STUFF`
172	`unsigned int linear_windowed_chirp(short *pole)`	173	`unsigned int linear_windowed_chirp(short *pole)`
173	`{`	174	`{`
174	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`	175	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`
175		176
176	`static const float f0 = 5000; //starting frequency`	177	`static const float f0 = 5000; //starting frequency`
177	`static const float fmax = 13000; //ending frequency`	178	`static const float fmax = 13000; //ending frequency`
178	`static const float Tw = 0.0015;`	179	`static const float Tw = 0.0015;`
179	`static float k;`	180	`static float k;`
180		181
181	`unsigned int n=0;`	182	`unsigned int n=0;`
182	`double t;`	183	`double t;`
183	`unsigned int chirp_samples; // number of samples per period`	184	`unsigned int chirp_samples; // number of samples per period`
184		185
185	`k=2*(fmax-f0)/Tw;`	186	`k=2*(fmax-f0)/Tw;`
186	`chirp_samples = ceil(rate*Tw);`	187	`chirp_samples = ceil(rate*Tw);`
187		188
188	`for (n=0;n<=chirp_samples;n++)`	189	`for (n=0;n<=chirp_samples;n++)`
189	`{`	190	`{`
190	`t = (double) n / (double)rate;`	191	`t = (double) n / (double)rate;`
191	`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))) );`	192	`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))) );`
192	`}`	193	`}`
193	`return (chirp_samples);`	194	`return (chirp_samples);`
194	`}`	195	`}`
195		196
196	`int main(int argc, char *argv[])`	197	`int main(int argc, char *argv[])`
197	`{`	198	`{`
198	`snd_pcm_t playback_handle, capture_handle;`	199	`snd_pcm_t playback_handle, capture_handle;`
199	`int err;`	200	`int err;`
200	`snd_pcm_hw_params_t *hwparams;`	201	`snd_pcm_hw_params_t *hwparams;`
201	`snd_pcm_sw_params_t *swparams;`	202	`snd_pcm_sw_params_t *swparams;`
202		203
203	`long int correlationl, correlationr;`	204	`long int correlationl, correlationr;`
204	`int L_signal, R_signal;`	205	`int L_signal, R_signal;`
205	`short chirp, signal;`	206	`short chirp, signal;`
-		207	`float chirp_spect, echo_spect;`
206	`unsigned int i,j,m,n;`	208	`unsigned int i,j,m,n;`
207	`unsigned int delay[10]; //store delay of signifed correlation`	209	`unsigned int delayl[10],delayr[10]; //store delay of signifed correlation`
208	`long int l,r; // store correlation at strict time`	210	`long int l,r; // store correlation at strict time`
-		211	`double df; //frequency resolution`
-		212	`unsigned int frequency_bins; // number of output frequency bins`
-		213
-		214	`double *inchirp;`
-		215	`fftw_complex *outchirp;`
-		216	`fftw_plan fft_plan_chirp;`
209		217
210	`FILE *out;`	218	`FILE *out;`
211		219
212	`snd_pcm_hw_params_alloca(&hwparams);`	220	`snd_pcm_hw_params_alloca(&hwparams);`
213	`snd_pcm_sw_params_alloca(&swparams);`	221	`snd_pcm_sw_params_alloca(&swparams);`
214		222
215	`printf("Simple PC sonar ver. 000000001 starting work.. \n");`	223	`printf("Simple PC sonar ver. 000000001 starting work.. \n");`
216		224
217	`//open and set playback device`	225	`//open and set playback device`
218	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`	226	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`
219	`{`	227	`{`
220	`printf("Playback open error: %s\n", snd_strerror(err));`	228	`printf("Playback open error: %s\n", snd_strerror(err));`
221	`return 0;`	229	`return 0;`
222	`}`	230	`}`
223		231
224	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`	232	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`
225	`{`	233	`{`
226	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	234	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
227	`exit(EXIT_FAILURE);`	235	`exit(EXIT_FAILURE);`
228	`}`	236	`}`
229	`if ((err = set_swparams(playback_handle, swparams)) < 0)`	237	`if ((err = set_swparams(playback_handle, swparams)) < 0)`
230	`{`	238	`{`
231	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	239	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
232	`exit(EXIT_FAILURE);`	240	`exit(EXIT_FAILURE);`
233	`}`	241	`}`
234		242
235	`//open and set capture device`	243	`//open and set capture device`
236	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`	244	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`
237	`{`	245	`{`
238	`printf("Playback open error: %s\n", snd_strerror(err));`	246	`printf("Playback open error: %s\n", snd_strerror(err));`
239	`return 0;`	247	`return 0;`
240	`}`	248	`}`
241		249
242	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`	250	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`
243	`{`	251	`{`
244	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	252	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
245	`exit(EXIT_FAILURE);`	253	`exit(EXIT_FAILURE);`
246	`}`	254	`}`
247	`if ((err = set_swparams(capture_handle, swparams)) < 0)`	255	`if ((err = set_swparams(capture_handle, swparams)) < 0)`
248	`{`	256	`{`
249	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	257	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
250	`exit(EXIT_FAILURE);`	258	`exit(EXIT_FAILURE);`
251	`}`	259	`}`
252		260
253	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together`	261	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together`
254	`if (err < 0)`	262	`if (err < 0)`
255	`{`	263	`{`
256	`printf("Device linking error: %s\n", snd_strerror(err));`	264	`printf("Device linking error: %s\n", snd_strerror(err));`
257	`exit(EXIT_FAILURE);`	265	`exit(EXIT_FAILURE);`
258	`}*/`	266	`}*/`
259		267
260	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`	268	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`
261	`correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve`	269	`correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve`
262	`L_signal = malloc(period_size * sizeof(int));`	270	`L_signal = malloc(period_size * sizeof(int));`
263	`R_signal = malloc(period_size * sizeof(int));`	271	`R_signal = malloc(period_size * sizeof(int));`
264	`chirp = calloc(2*period_size, sizeof(short));`	272	`chirp = calloc(2*period_size, sizeof(short));`
265	`signal = malloc(2period_size sizeof(short));`	273	`signal = malloc(2period_size sizeof(short));`
266		274
267	`// generate ping pattern`	275	`// generate ping pattern`
268	`chirp_size = linear_windowed_chirp(chirp);`	276	`chirp_size = linear_windowed_chirp(chirp);`
269		277
-		278	`frequency_bins = chirp_size / 2 + 1;`
-		279	`df = (double) rate / (double) chirp_size;`
-		280	`chirp_spect = malloc(frequency_bins * sizeof(float));`
-		281	`echo_spect = malloc(frequency_bins * sizeof(float));`
-		282
-		283	`inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT`
-		284	`outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);`
-		285
-		286	`fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);`
-		287
-		288	`printf("compute chirp spectrum\n");`
-		289	`for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];`
-		290	`fftw_execute(fft_plan_chirp);`
-		291	`for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );`
-		292
-		293	`// write chirp data to souncard buffer`
270	`err = snd_pcm_writei(playback_handle, chirp, period_size);`	294	`err = snd_pcm_writei(playback_handle, chirp, period_size);`
271	`if (err < 0)`	295	`if (err < 0)`
272	`{`	296	`{`
273	`printf("Initial write error: %s\n", snd_strerror(err));`	297	`printf("Initial write error: %s\n", snd_strerror(err));`
274	`exit(EXIT_FAILURE);`	298	`exit(EXIT_FAILURE);`
275	`}`	299	`}`
276		300
277	`//start sream`	301	`//start sream`
278	`err = snd_pcm_start(playback_handle);`	302	`err = snd_pcm_start(playback_handle);`
279	`if (err < 0)`	303	`if (err < 0)`
280	`{`	304	`{`
281	`printf("Start error: %s\n", snd_strerror(err));`	305	`printf("Start error: %s\n", snd_strerror(err));`
282	`exit(EXIT_FAILURE);`	306	`exit(EXIT_FAILURE);`
283	`}`	307	`}`
284		308
285	`err = snd_pcm_start(capture_handle);`	309	`err = snd_pcm_start(capture_handle);`
286	`if (err < 0)`	310	`if (err < 0)`
287	`{`	311	`{`
288	`printf("Start error: %s\n", snd_strerror(err));`	312	`printf("Start error: %s\n", snd_strerror(err));`
289	`exit(EXIT_FAILURE);`	313	`exit(EXIT_FAILURE);`
290	`}`	314	`}`
291	`else printf("Waiting for transmitt all samples\n");`	315	`else printf("Waiting for transmitt all samples\n");`
292	`//--------------`	316	`//--------------`
293		-
294	`while ( snd_pcm_avail_update(capture_handle) < period_size)`	317	`while ( snd_pcm_avail_update(capture_handle) < period_size)`
295	`{`	318	`{`
296	`usleep(1000);`	319	`usleep(1000);`
297	`printf(".");`	320	`printf(".");`
298	`}`	321	`}`
299		322
300	`err = snd_pcm_drop(playback_handle);`	323	`err = snd_pcm_drop(playback_handle);`
301	`err = snd_pcm_drain(capture_handle);`	324	`err = snd_pcm_drain(capture_handle);`
302	`if (err < 0)`	325	`if (err < 0)`
303	`{`	326	`{`
304	`printf("Stop error: %s\n", snd_strerror(err));`	327	`printf("Stop error: %s\n", snd_strerror(err));`
305	`exit(EXIT_FAILURE);`	328	`exit(EXIT_FAILURE);`
306	`}`	329	`}`
307		330
308	`err = snd_pcm_readi(capture_handle, signal, period_size);`	331	`err = snd_pcm_readi(capture_handle, signal, period_size);`
309	`if (err < 0)`	332	`if (err < 0)`
310	`{`	333	`{`
311	`printf("Read error: %s\n", snd_strerror(err));`	334	`printf("Read error: %s\n", snd_strerror(err));`
312	`exit(EXIT_FAILURE);`	335	`exit(EXIT_FAILURE);`
313	`}`	336	`}`
314		337
315	`j=0;`	338	`j=0;`
316	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`	339	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`
317	`{`	340	`{`
318	`L_signal[i]=signal[j];`	341	`L_signal[i]=signal[j];`
319	`R_signal[i]=signal[j+1];`	342	`R_signal[i]=signal[j+1];`
320	`j+=2;`	343	`j+=2;`
321	`}`	344	`}`
322		345
323	`printf("\nData transmitted \ncorrelating\n");`	346	`printf("\nData transmitted \ncorrelating\n");`
324	`for (n=0; n < (period_size - chirp_size - 1); n++)`	347	`for (n=0; n < (period_size - chirp_size - 1); n++)`
325	`{`	348	`{`
326	`l=0;`	349	`l=0;`
327	`r=0;`	350	`r=0;`
328	`for (m=0;m < chirp_size;m++)`	351	`for ( m = 0; m < chirp_size;m++)`
329	`{`	352	`{`
330	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`	353	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`
331	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`	354	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`
332	`}`	355	`}`
333	`correlationl[n]=l;`	356	`correlationl[n]=abs(l);`
334	`correlationr[n]=r;`	357	`correlationr[n]=abs(r);`
335	`}`	358	`}`
336		359
337	`printf("Searching echos\n");`	360	`printf("Searching echos\n");`
338	`r=0;`	361	`r=0;`
339	`l=0;`	362	`l=0;`
340	`for (n=0; n < period_size;n++) //najde nejvetsi korelace`	363	`for (n=0; n < period_size;n++) //najde nejvetsi korelace`
341	`{`	364	`{`
342	`if (l < correlationl[n])`	365	`if (l < correlationl[n])`
343	`{`	366	`{`
344	`delay[1] = n;`	367	`delayl[1] = n;`
345	`l = correlationl[n];`	368	`l = correlationl[n];`
346	`}`	369	`}`
347	`if (r < correlationr[n])`	370	`if (r < correlationr[n])`
348	`{`	371	`{`
349	`delay[2] = n;`	372	`delayr[1] = n;`
350	`r = correlationr[n];`	373	`r = correlationr[n];`
351	`}`	374	`}`
352	`}`	375	`}`
353		376
-		377
-		378
354	`printf("Writing output file\n");`	379	`printf("Writing output file\n");`
355	`out=fopen("/tmp/sonar.txt","w");`	380	`out=fopen("/tmp/sonar.txt","w");`
356	`j=0;`	381	`j=0;`
357	`for (i=0; i <= (period_size -1); i++)`	382	`for (i=0; i <= (period_size - 1); i++)`
-		383	`{`
-		384	`fprintf(out,"%6d %6d %6d %9ld %9ld\n",i,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);`
-		385	`j+=2;`
-		386	`}`
-		387	`fclose(out);`
-		388
-		389	`out=fopen("/tmp/chirp.txt","w");`
-		390	`j=0;`
-		391	`for (i=0; i <= (chirp_size - 1); i++)`
358	`{`	392	`{`
359	`fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]);`	393	`fprintf(out,"%6d %6d %f\n", i, chirp[i], chirp_spect[i]);`
360	`j+=2;`	394	`j+=2;`
361	`}`	395	`}`
362	`fclose(out);`	396	`fclose(out);`
363		397
364	`printf("Echo zacina na: %d vzorku.\n", delay[1]);`	398	`printf("Echo zacina na: %d vzorku.\n", delayl[1]);`
365	`printf("Casove na: %f s\n", ((float)delay[1]/rate));`	399	`printf("Casove na: %f s\n", ((float)delayl[1]/rate));`
366	`printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delay[1]/rate));`	400	`printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delayl[1]/rate));`
-		401
-		402	`free(correlationl);`
-		403	`free(correlationr);`
-		404	`free(L_signal);`
-		405	`free(R_signal);`
-		406	`free(chirp);`
-		407	`free(signal);`
367		408
368	`snd_pcm_close(playback_handle);`	409	`snd_pcm_close(playback_handle);`
369	`snd_pcm_close(capture_handle);`	410	`snd_pcm_close(capture_handle);`
370	`return 0;`	411	`return 0;`
371	`}`	412	`}`
372		413

Subversion Repositories svnkaklik

(root)/programy/C/ix86/echo/SW/sonar/src/sonar.c @ 564 – Rev 562 → 563