WebSVN – svnkaklik – Diff – /programy/C/ix86/signals/alsa/sonar/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	5.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)  // 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))) ); // ping 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;		//variables for driver handlers
    int err;
    snd_pcm_hw_params_t *hwparams;		// hardware and software parameters arrays
    snd_pcm_sw_params_t *swparams;
 
    long int *correlationl, *correlationr;	// pointers to arrays where correlation will be stored
    float k;
    int *L_signal, *R_signal;	// array of captured data from left and right channel
    short *chirp, *signal;	// chirp and soundcard buffer output data
    unsigned int i,j,m,n;
    unsigned int map_size; //number of points in echo map.
    long int l,r;  // store correlation at strict time
 
    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 seems doesn't work 
        if (err < 0)
        {
            printf("Device linking error: %s\n", snd_strerror(err));
            exit(EXIT_FAILURE);
        }*/
 
    k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance
 
    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);
 
// write generated 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 until one period of data is transmitted
    {
        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 whole 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);
    }
 
    printf("Writing output files\n");
    out=fopen("/tmp/sonar.txt","w");		// save captured and computed correlation data for both channels
    for (i=0; i <= (period_size - 1); i++)
    {
        fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);
    }
    fclose(out);
 
    out=fopen("/tmp/chirp.txt","w");		// save chirp data to someone who want it
    for (i=0; i <= (chirp_size - 1); i++)
    {
        fprintf(out,"%6d %6d\n", i, chirp[i]);
    }
    fclose(out);
 
    printf("Job done.\n");
 
				//free all arrays 
    free(correlationl);
    free(correlationr);
    free(L_signal);
    free(R_signal);
    free(chirp);
    free(signal);
 
    snd_pcm_close(playback_handle);	// free driver handlers 
    snd_pcm_close(capture_handle);
    return 0;
}
 

Rev 662	Rev 663
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 5.0 // maximal working radius in meters`	22	`#define MAX_RANGE 5.0 // maximal working radius in meters`
23		23
24	`static char device = "plughw:0,0"; / playback device */`	24	`static char device = "plughw:0,0"; / playback device */`
25	`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 */`
26	`static unsigned int rate = 96000; /* stream rate */`	26	`static unsigned int rate = 96000; /* stream rate */`
27	`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 */`
28	`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 */`
29	`static int resample = 1; /* enable alsa-lib resampling */`	29	`static int resample = 1; /* enable alsa-lib resampling */`
30		30
31	`unsigned int chirp_size;`	31	`unsigned int chirp_size;`
32		32
33	`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`
34	`static snd_pcm_sframes_t period_size; //samples per frame`	34	`static snd_pcm_sframes_t period_size; //samples per frame`
35	`static snd_output_t *output = NULL;`	35	`static snd_output_t *output = NULL;`
36		36
37	`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)`
38	`{`	38	`{`
39	`unsigned int rrate;`	39	`unsigned int rrate;`
40	`snd_pcm_uframes_t size;`	40	`snd_pcm_uframes_t size;`
41	`int err, dir;`	41	`int err, dir;`
42		42
43	`/* choose all parameters */`	43	`/* choose all parameters */`
44	`err = snd_pcm_hw_params_any(handle, params);`	44	`err = snd_pcm_hw_params_any(handle, params);`
45	`if (err < 0)`	45	`if (err < 0)`
46	`{`	46	`{`
47	`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));`
48	`return err;`	48	`return err;`
49	`}`	49	`}`
50	`/* set hardware resampling */`	50	`/* set hardware resampling */`
51	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`	51	`err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);`
52	`if (err < 0)`	52	`if (err < 0)`
53	`{`	53	`{`
54	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`	54	`printf("Resampling setup failed for playback: %s\n", snd_strerror(err));`
55	`return err;`	55	`return err;`
56	`}`	56	`}`
57	`/* set the interleaved read/write format */`	57	`/* set the interleaved read/write format */`
58	`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);`
59	`if (err < 0)`	59	`if (err < 0)`
60	`{`	60	`{`
61	`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));`
62	`return err;`	62	`return err;`
63	`}`	63	`}`
64	`/* set the sample format */`	64	`/* set the sample format */`
65	`err = snd_pcm_hw_params_set_format(handle, params, format);`	65	`err = snd_pcm_hw_params_set_format(handle, params, format);`
66	`if (err < 0)`	66	`if (err < 0)`
67	`{`	67	`{`
68	`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));`
69	`return err;`	69	`return err;`
70	`}`	70	`}`
71	`/* set the count of channels */`	71	`/* set the count of channels */`
72	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`	72	`err = snd_pcm_hw_params_set_channels(handle, params, channels);`
73	`if (err < 0)`	73	`if (err < 0)`
74	`{`	74	`{`
75	`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));`
76	`return err;`	76	`return err;`
77	`}`	77	`}`
78	`/* set the stream rate */`	78	`/* set the stream rate */`
79	`rrate = rate;`	79	`rrate = rate;`
80	`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);`
81	`if (err < 0)`	81	`if (err < 0)`
82	`{`	82	`{`
83	`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));`
84	`return err;`	84	`return err;`
85	`}`	85	`}`
86	`if (rrate != rate)`	86	`if (rrate != rate)`
87	`{`	87	`{`
88	`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);`
89	`return -EINVAL;`	89	`return -EINVAL;`
90	`}`	90	`}`
91	`else printf("Rate set to %i Hz\n", rate, err);`	91	`else printf("Rate set to %i Hz\n", rate, err);`
92	`/* set the buffer time */`	92	`/* set the buffer time */`
93	`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);`
94	`if (err < 0)`	94	`if (err < 0)`
95	`{`	95	`{`
96	`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));`
97	`return err;`	97	`return err;`
98	`}`	98	`}`
99	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`	99	`err = snd_pcm_hw_params_get_buffer_size(params, &size);`
100	`if (err < 0)`	100	`if (err < 0)`
101	`{`	101	`{`
102	`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));`
103	`return err;`	103	`return err;`
104	`}`	104	`}`
105	`buffer_size = size;`	105	`buffer_size = size;`
106	`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);`
107		107
108		108
109	`// set the period time`	109	`// set the period time`
110	`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);`
111	`if (err < 0)`	111	`if (err < 0)`
112	`{`	112	`{`
113	`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));`
114	`return err;`	114	`return err;`
115	`}`	115	`}`
116		116
117	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`	117	`err = snd_pcm_hw_params_get_period_size(params, &size, &dir);`
118	`if (err < 0)`	118	`if (err < 0)`
119	`{`	119	`{`
120	`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));`
121	`return err;`	121	`return err;`
122	`}`	122	`}`
123	`period_size = size;`	123	`period_size = size;`
124	`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);`
125		125
126	`/* write the parameters to device */`	126	`/* write the parameters to device */`
127	`err = snd_pcm_hw_params(handle, params);`	127	`err = snd_pcm_hw_params(handle, params);`
128	`if (err < 0)`	128	`if (err < 0)`
129	`{`	129	`{`
130	`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));`
131	`return err;`	131	`return err;`
132	`}`	132	`}`
133	`return 0;`	133	`return 0;`
134	`}`	134	`}`
135		135
136	`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)`
137	`{`	137	`{`
138	`int err;`	138	`int err;`
139		139
140	`/* get the current swparams */`	140	`/* get the current swparams */`
141	`err = snd_pcm_sw_params_current(handle, swparams);`	141	`err = snd_pcm_sw_params_current(handle, swparams);`
142	`if (err < 0)`	142	`if (err < 0)`
143	`{`	143	`{`
144	`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));`
145	`return err;`	145	`return err;`
146	`}`	146	`}`
147	`// 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`
148	`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);`
149	`if (err < 0)`	149	`if (err < 0)`
150	`{`	150	`{`
151	`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));`
152	`return err;`	152	`return err;`
153	`}`	153	`}`
154		154
155	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`	155	`err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);`
156	`if (err < 0)`	156	`if (err < 0)`
157	`{`	157	`{`
158	`printf("Unable to set period event: %s\n", snd_strerror(err));`	158	`printf("Unable to set period event: %s\n", snd_strerror(err));`
159	`return err;`	159	`return err;`
160	`}`	160	`}`
161		161
162	`/* write the parameters to the playback device */`	162	`/* write the parameters to the playback device */`
163	`err = snd_pcm_sw_params(handle, swparams);`	163	`err = snd_pcm_sw_params(handle, swparams);`
164	`if (err < 0)`	164	`if (err < 0)`
165	`{`	165	`{`
166	`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));`
167	`return err;`	167	`return err;`
168	`}`	168	`}`
169	`return 0;`	169	`return 0;`
170	`}`	170	`}`
171		171
172	`////// SIGNAL GENERATION STUFF`	172	`////// SIGNAL GENERATION STUFF`
173	`unsigned int linear_windowed_chirp(short *pole) // generate the ping signal`	173	`unsigned int linear_windowed_chirp(short *pole) // generate the ping signal`
174	`{`	174	`{`
175	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`	175	`unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;`
176		176
177	`static const float f0 = 5000; //starting frequency`	177	`static const float f0 = 5000; //starting frequency`
178	`static const float fmax = 10000; //ending frequency`	178	`static const float fmax = 10000; //ending frequency`
179	`static const float Tw = 0.0015; // time width of ping in seconds`	179	`static const float Tw = 0.0015; // time width of ping in seconds`
180	`static float k;`	180	`static float k;`
181		181
182	`unsigned int n=0;`	182	`unsigned int n=0;`
183	`double t;`	183	`double t;`
184	`unsigned int chirp_samples; // number of samples per period`	184	`unsigned int chirp_samples; // number of samples per period`
185		185
186	`k=2*(fmax-f0)/Tw;`	186	`k=2*(fmax-f0)/Tw;`
187	`chirp_samples = ceil(rate*Tw); // compute size of ping sinal in samples`	187	`chirp_samples = ceil(rate*Tw); // compute size of ping sinal in samples`
188		188
189	`for (n=0;n<=chirp_samples;n++)`	189	`for (n=0;n<=chirp_samples;n++)`
190	`{`	190	`{`
191	`t = (double) n / (double)rate;`	191	`t = (double) n / (double)rate;`
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))) ); // signal generation formula`	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))) ); // ping signal generation formula`
193	`}`	193	`}`
194	`return (chirp_samples); // return count of samples in ping`	194	`return (chirp_samples); // return count of samples in ping`
195	`}`	195	`}`
196		196
197	`int main(int argc, char *argv[])`	197	`int main(int argc, char *argv[])`
198	`{`	198	`{`
199	`snd_pcm_t playback_handle, capture_handle;`	199	`snd_pcm_t playback_handle, capture_handle; //variables for driver handlers`
200	`int err;`	200	`int err;`
201	`snd_pcm_hw_params_t *hwparams;`	201	`snd_pcm_hw_params_t *hwparams; // hardware and software parameters arrays`
202	`snd_pcm_sw_params_t *swparams;`	202	`snd_pcm_sw_params_t *swparams;`
203		203
204	`long int correlationl, correlationr;`	204	`long int correlationl, correlationr; // pointers to arrays where correlation will be stored`
205	`float k;`	205	`float k;`
206	`int L_signal, R_signal;`	206	`int L_signal, R_signal; // array of captured data from left and right channel`
207	`short chirp, signal;`	207	`short chirp, signal; // chirp and soundcard buffer output data`
208	`unsigned int i,j,m,n;`	208	`unsigned int i,j,m,n;`
209	`unsigned int map_size; //number of points in echo map.`	209	`unsigned int map_size; //number of points in echo map.`
210	`long int l,r; // store correlation at strict time`	210	`long int l,r; // store correlation at strict time`
211		211
212	`FILE *out; // dummy variable for file data output`	212	`FILE *out; // dummy variable for file data output`
213		213
214	`snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registers`	214	`snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registers`
215	`snd_pcm_sw_params_alloca(&swparams);`	215	`snd_pcm_sw_params_alloca(&swparams);`
216		216
217	`printf("Simple PC sonar $Rev:$ starting work.. \n");`	217	`printf("Simple PC sonar $Rev:$ starting work.. \n");`
218		218
219	`//open and set playback device`	219	`//open and set playback device`
220	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`	220	`if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)`
221	`{`	221	`{`
222	`printf("Playback open error: %s\n", snd_strerror(err));`	222	`printf("Playback open error: %s\n", snd_strerror(err));`
223	`return 0;`	223	`return 0;`
224	`}`	224	`}`
225		225
226	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`	226	`if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)`
227	`{`	227	`{`
228	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	228	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
229	`exit(EXIT_FAILURE);`	229	`exit(EXIT_FAILURE);`
230	`}`	230	`}`
231	`if ((err = set_swparams(playback_handle, swparams)) < 0)`	231	`if ((err = set_swparams(playback_handle, swparams)) < 0)`
232	`{`	232	`{`
233	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	233	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
234	`exit(EXIT_FAILURE);`	234	`exit(EXIT_FAILURE);`
235	`}`	235	`}`
236		236
237	`//open and set capture device`	237	`//open and set capture device`
238	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`	238	`if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)`
239	`{`	239	`{`
240	`printf("Playback open error: %s\n", snd_strerror(err));`	240	`printf("Playback open error: %s\n", snd_strerror(err));`
241	`return 0;`	241	`return 0;`
242	`}`	242	`}`
243		243
244	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`	244	`if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)`
245	`{`	245	`{`
246	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`	246	`printf("Setting of hwparams failed: %s\n", snd_strerror(err));`
247	`exit(EXIT_FAILURE);`	247	`exit(EXIT_FAILURE);`
248	`}`	248	`}`
249	`if ((err = set_swparams(capture_handle, swparams)) < 0)`	249	`if ((err = set_swparams(capture_handle, swparams)) < 0)`
250	`{`	250	`{`
251	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`	251	`printf("Setting of swparams failed: %s\n", snd_strerror(err));`
252	`exit(EXIT_FAILURE);`	252	`exit(EXIT_FAILURE);`
253	`}`	253	`}`
254		254
255	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together seems doesn't work`	255	`/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together seems doesn't work`
256	`if (err < 0)`	256	`if (err < 0)`
257	`{`	257	`{`
258	`printf("Device linking error: %s\n", snd_strerror(err));`	258	`printf("Device linking error: %s\n", snd_strerror(err));`
259	`exit(EXIT_FAILURE);`	259	`exit(EXIT_FAILURE);`
260	`}*/`	260	`}*/`
261		261
262	`k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance`	262	`k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance`
263		263
264	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`	264	`correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve`
265	`correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve`	265	`correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve`
266	`L_signal = malloc(period_size * sizeof(int));`	266	`L_signal = malloc(period_size * sizeof(int));`
267	`R_signal = malloc(period_size * sizeof(int));`	267	`R_signal = malloc(period_size * sizeof(int));`
268	`chirp = calloc(2*period_size, sizeof(short));`	268	`chirp = calloc(2*period_size, sizeof(short));`
269	`signal = malloc(2period_size sizeof(short));`	269	`signal = malloc(2period_size sizeof(short));`
270		270
271	`// generate ping pattern`	271	`// generate ping pattern`
272	`chirp_size = linear_windowed_chirp(chirp);`	272	`chirp_size = linear_windowed_chirp(chirp);`
273		273
274	`// write generated chirp data to souncard buffer`	274	`// write generated chirp data to souncard buffer`
275	`err = snd_pcm_writei(playback_handle, chirp, period_size);`	275	`err = snd_pcm_writei(playback_handle, chirp, period_size);`
276	`if (err < 0)`	276	`if (err < 0)`
277	`{`	277	`{`
278	`printf("Initial write error: %s\n", snd_strerror(err));`	278	`printf("Initial write error: %s\n", snd_strerror(err));`
279	`exit(EXIT_FAILURE);`	279	`exit(EXIT_FAILURE);`
280	`}`	280	`}`
281		281
282	`//start sream`	282	`//start sream`
283	`err = snd_pcm_start(playback_handle);`	283	`err = snd_pcm_start(playback_handle);`
284	`if (err < 0)`	284	`if (err < 0)`
285	`{`	285	`{`
286	`printf("Start error: %s\n", snd_strerror(err));`	286	`printf("Start error: %s\n", snd_strerror(err));`
287	`exit(EXIT_FAILURE);`	287	`exit(EXIT_FAILURE);`
288	`}`	288	`}`
289		289
290	`err = snd_pcm_start(capture_handle);`	290	`err = snd_pcm_start(capture_handle);`
291	`if (err < 0)`	291	`if (err < 0)`
292	`{`	292	`{`
293	`printf("Start error: %s\n", snd_strerror(err));`	293	`printf("Start error: %s\n", snd_strerror(err));`
294	`exit(EXIT_FAILURE);`	294	`exit(EXIT_FAILURE);`
295	`}`	295	`}`
296	`else printf("Transmitting all samples of chirp\n");`	296	`else printf("Transmitting all samples of chirp\n");`
297	`//--------------`	297	`//--------------`
298		298
299	`while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data`	299	`while ( snd_pcm_avail_update(capture_handle) < period_size) // wait until one period of data is transmitted`
300	`{`	300	`{`
301	`usleep(1000);`	301	`usleep(1000);`
302	`printf(".");`	302	`printf(".");`
303	`}`	303	`}`
304		304
305	`err = snd_pcm_drop(playback_handle); // stop audio stream`	305	`err = snd_pcm_drop(playback_handle); // stop audio stream`
306	`err = snd_pcm_drain(capture_handle);`	306	`err = snd_pcm_drain(capture_handle);`
307	`if (err < 0)`	307	`if (err < 0)`
308	`{`	308	`{`
309	`printf("Stop error: %s\n", snd_strerror(err));`	309	`printf("Stop error: %s\n", snd_strerror(err));`
310	`exit(EXIT_FAILURE);`	310	`exit(EXIT_FAILURE);`
311	`}`	311	`}`
312		312
313	`err = snd_pcm_readi(capture_handle, signal, period_size); //read whole period from audio buffer`	313	`err = snd_pcm_readi(capture_handle, signal, period_size); //read whole period from audio buffer`
314	`if (err < 0)`	314	`if (err < 0)`
315	`{`	315	`{`
316	`printf("Read error: %s\n", snd_strerror(err));`	316	`printf("Read error: %s\n", snd_strerror(err));`
317	`exit(EXIT_FAILURE);`	317	`exit(EXIT_FAILURE);`
318	`}`	318	`}`
319		319
320	`j=0;`	320	`j=0;`
321	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`	321	`for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays`
322	`{`	322	`{`
323	`L_signal[i]=signal[j];`	323	`L_signal[i]=signal[j];`
324	`R_signal[i]=signal[j+1];`	324	`R_signal[i]=signal[j+1];`
325	`j+=2;`	325	`j+=2;`
326	`}`	326	`}`
327		327
328	`printf("\nChirp transmitted \ncorrelating\n");`	328	`printf("\nChirp transmitted \ncorrelating\n");`
329	`for (n=0; n < (period_size - chirp_size - 1); n++)`	329	`for (n=0; n < (period_size - chirp_size - 1); n++)`
330	`{`	330	`{`
331	`l=0;`	331	`l=0;`
332	`r=0;`	332	`r=0;`
333	`for ( m = 0; m < chirp_size;m++)`	333	`for ( m = 0; m < chirp_size;m++)`
334	`{`	334	`{`
335	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`	335	`l += chirp[m]*L_signal[m+n]; // correlate with left channel`
336	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`	336	`r += chirp[m]*R_signal[m+n]; // correlate with right channel`
337	`}`	337	`}`
338	`correlationl[n]=abs(l);`	338	`correlationl[n]=abs(l);`
339	`correlationr[n]=abs(r);`	339	`correlationr[n]=abs(r);`
340	`}`	340	`}`
341		341
342	`printf("Writing output files\n");`	342	`printf("Writing output files\n");`
343	`out=fopen("/tmp/sonar.txt","w");`	343	`out=fopen("/tmp/sonar.txt","w"); // save captured and computed correlation data for both channels`
344	`for (i=0; i <= (period_size - 1); i++)`	344	`for (i=0; i <= (period_size - 1); i++)`
345	`{`	345	`{`
346	`fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);`	346	`fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);`
347	`}`	347	`}`
348	`fclose(out);`	348	`fclose(out);`
349		349
350	`out=fopen("/tmp/chirp.txt","w");`	350	`out=fopen("/tmp/chirp.txt","w"); // save chirp data to someone who want it`
351	`for (i=0; i <= (chirp_size - 1); i++)`	351	`for (i=0; i <= (chirp_size - 1); i++)`
352	`{`	352	`{`
353	`fprintf(out,"%6d %6d\n", i, chirp[i]);`	353	`fprintf(out,"%6d %6d\n", i, chirp[i]);`
354	`}`	354	`}`
355	`fclose(out);`	355	`fclose(out);`
356		356
357	`printf("Job done.\n");`	357	`printf("Job done.\n");`
358		358
-		359	`//free all arrays`
359	`free(correlationl);`	360	`free(correlationl);`
360	`free(correlationr);`	361	`free(correlationr);`
361	`free(L_signal);`	362	`free(L_signal);`
362	`free(R_signal);`	363	`free(R_signal);`
363	`free(chirp);`	364	`free(chirp);`
364	`free(signal);`	365	`free(signal);`
365		366
366	`snd_pcm_close(playback_handle);`	367	`snd_pcm_close(playback_handle); // free driver handlers`
367	`snd_pcm_close(capture_handle);`	368	`snd_pcm_close(capture_handle);`
368	`return 0;`	369	`return 0;`
369	`}`	370	`}`
370		371

Subversion Repositories svnkaklik

(root)/programy/C/ix86/signals/alsa/sonar/sonar.c – Rev 662 → 663