Subversion Repositories svnkaklik

Rev

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

Rev 560 Rev 561
Line 15... Line 15...
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
 
19
 
-
 
20
#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
 
20
static char *device = "plughw:0,0";			/* playback device */
23
static char *device = "plughw:0,0";			/* playback device */
21
static snd_pcm_format_t format = SND_PCM_FORMAT_S16;	/* sample format */
24
static snd_pcm_format_t format = SND_PCM_FORMAT_S16;	/* sample format */
22
static unsigned int rate = 98000;			/* stream rate */
25
static unsigned int rate = 96000;			/* stream rate */
23
static unsigned int buffer_time = 500000;		/* ring buffer length in us */
26
static unsigned int buffer_time = MAX_RANGE / SOUND_SPEED * 1e6;		/* ring buffer length in us */
24
static unsigned int period_time = 100000;		/* period time in us */
27
static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e5;		/* period time in us */
25
static int resample = 1;				/* enable alsa-lib resampling */
28
static int resample = 1;				/* enable alsa-lib resampling */
26
static int period_event = 0;				/* produce poll event after each period */
-
 
27
 
29
 
28
#define SOUND_SPEED	340
-
 
29
#define SIGNAL_SAMPLES 100000
30
#define SIGNAL_SAMPLES 100000
30
#define CHIRP_OFFSET	0 
-
 
31
 
31
 
32
unsigned int chirp_size;
32
unsigned int chirp_size;
33
 
33
 
34
int period=0;
34
int period=0;
35
int cperiod=0;
35
int cperiod=0;
36
int chirp[100000];
36
short *chirp;
37
short signal[1000000];		// record 6s of input samples
37
short signal[1000000];		// record 6s of input samples
38
 
38
 
39
static snd_pcm_sframes_t buffer_size;	// size of buffer at sound card
39
static snd_pcm_sframes_t buffer_size;	// size of buffer at sound card
40
static snd_pcm_sframes_t period_size;	//samples per frame
40
static snd_pcm_sframes_t period_size;	//samples per frame
41
static snd_output_t *output = NULL;
41
static snd_output_t *output = NULL;
42
 
42
 
43
static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels)
43
static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels)
44
{
44
{
45
	unsigned int rrate;
45
    unsigned int rrate;
46
	snd_pcm_uframes_t size;
46
    snd_pcm_uframes_t size;
47
	int err, dir;
47
    int err, dir;
48
 
48
 
49
	/* choose all parameters */
49
    /* choose all parameters */
50
	err = snd_pcm_hw_params_any(handle, params);
50
    err = snd_pcm_hw_params_any(handle, params);
51
	if (err < 0) {
51
    if (err < 0)
-
 
52
    {
52
		printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));
53
        printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));
53
		return err;
54
        return err;
54
	}
55
    }
55
	/* set hardware resampling */
56
    /* set hardware resampling */
56
	err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);
57
    err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);
57
	if (err < 0) {
58
    if (err < 0)
-
 
59
    {
58
		printf("Resampling setup failed for playback: %s\n", snd_strerror(err));
60
        printf("Resampling setup failed for playback: %s\n", snd_strerror(err));
59
		return err;
61
        return err;
60
	}
62
    }
61
	/* set the interleaved read/write format */
63
    /* set the interleaved read/write format */
62
	err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);
64
    err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);
63
	if (err < 0) {
65
    if (err < 0)
-
 
66
    {
64
		printf("Access type not available for playback: %s\n", snd_strerror(err));
67
        printf("Access type not available for playback: %s\n", snd_strerror(err));
65
		return err;
68
        return err;
66
	}
69
    }
67
	/* set the sample format */
70
    /* set the sample format */
68
	err = snd_pcm_hw_params_set_format(handle, params, format);
71
    err = snd_pcm_hw_params_set_format(handle, params, format);
69
	if (err < 0) {
72
    if (err < 0)
-
 
73
    {
70
		printf("Sample format not available for playback: %s\n", snd_strerror(err));
74
        printf("Sample format not available for playback: %s\n", snd_strerror(err));
71
		return err;
75
        return err;
72
	}
76
    }
73
	/* set the count of channels */
77
    /* set the count of channels */
74
	err = snd_pcm_hw_params_set_channels(handle, params, channels);
78
    err = snd_pcm_hw_params_set_channels(handle, params, channels);
75
	if (err < 0) {
79
    if (err < 0)
-
 
80
    {
76
		printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));
81
        printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));
77
		return err;
82
        return err;
78
	}
83
    }
79
	/* set the stream rate */
84
    /* set the stream rate */
80
	rrate = rate;
85
    rrate = rate;
81
	err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);
86
    err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);
82
	if (err < 0) {
87
    if (err < 0)
-
 
88
    {
83
		printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));
89
        printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));
84
		return err;
90
        return err;
85
	}
91
    }
86
	if (rrate != rate) {
92
    if (rrate != rate)
-
 
93
    {
87
		printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);
94
        printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);
88
		return -EINVAL;
95
        return -EINVAL;
89
	}
96
    }
90
	else printf("Rate set to %i Hz\n", rate, err); 
97
    else printf("Rate set to %i Hz\n", rate, err);
91
	/* set the buffer time */
98
    /* set the buffer time */
92
	err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);
99
    err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);
93
	if (err < 0) {
100
    if (err < 0)
-
 
101
    {
94
		printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));
102
        printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));
95
		return err;
103
        return err;
96
	}
104
    }
97
	err = snd_pcm_hw_params_get_buffer_size(params, &size);
105
    err = snd_pcm_hw_params_get_buffer_size(params, &size);
98
	if (err < 0) {
106
    if (err < 0)
-
 
107
    {
99
		printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));
108
        printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));
100
		return err;
109
        return err;
101
	}
110
    }
102
	buffer_size = size;
111
    buffer_size = size;
-
 
112
    printf("Bufffer size set to:  %d  Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);
-
 
113
 
-
 
114
 
103
	/* set the period time */
115
    /// set the period time
104
	err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);
116
    err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);
105
	if (err < 0) {
117
    if (err < 0)
-
 
118
    {
106
		printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));
119
        printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));
107
		return err;
120
        return err;
-
 
121
    }
108
	}
122
 
109
	err = snd_pcm_hw_params_get_period_size(params, &size, &dir);
123
    err = snd_pcm_hw_params_get_period_size(params, &size, &dir);
110
	if (err < 0) {
124
    if (err < 0)
-
 
125
    {
111
		printf("Unable to get period size for playback: %s\n", snd_strerror(err));
126
        printf("Unable to get period size for playback: %s\n", snd_strerror(err));
112
		return err;
127
        return err;
113
	}
128
    }
114
	period_size = size;
129
    period_size = size;
-
 
130
    printf("Period size set to:  %d Requested period time: %ld \n", (int) period_size, (long) period_time);
-
 
131
 
115
	/* write the parameters to device */
132
    /* write the parameters to device */
116
	err = snd_pcm_hw_params(handle, params);
133
    err = snd_pcm_hw_params(handle, params);
117
	if (err < 0) {
134
    if (err < 0)
-
 
135
    {
118
		printf("Unable to set hw params for playback: %s\n", snd_strerror(err));
136
        printf("Unable to set hw params for playback: %s\n", snd_strerror(err));
119
		return err;
137
        return err;
120
	}
138
    }
121
	return 0;
139
    return 0;
122
}
140
}
123
 
141
 
124
static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams)
142
static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams)
125
{
143
{
126
	int err;
144
    int err;
127
 
145
 
128
	/* get the current swparams */
146
    /* get the current swparams */
129
	err = snd_pcm_sw_params_current(handle, swparams);
147
    err = snd_pcm_sw_params_current(handle, swparams);
130
	if (err < 0) {
148
    if (err < 0)
-
 
149
    {
131
		printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));
150
        printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));
132
		return err;
151
        return err;
133
	}
152
    }
134
	/* start the transfer when the buffer is almost full: */
153
    // start the transfer when the buffer is almost full: never fou our case 
135
	/* (buffer_size / avail_min) * avail_min */
-
 
136
	err = snd_pcm_sw_params_set_start_threshold(handle, swparams, (buffer_size / period_size) * period_size);
154
    err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);
137
	if (err < 0) {
155
    if (err < 0)
-
 
156
    {
138
		printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));
157
        printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));
139
		return err;
158
        return err;
140
	}
159
    }
141
	/* allow the transfer when at least period_size samples can be processed */
-
 
142
	/* or disable this mechanism when period event is enabled (aka interrupt like style processing) */
-
 
143
	err = snd_pcm_sw_params_set_avail_min(handle, swparams, period_event ? buffer_size : period_size);
-
 
144
	if (err < 0) {
-
 
145
		printf("Unable to set avail min for playback: %s\n", snd_strerror(err));
-
 
146
		return err;
-
 
147
	}
160
 
148
	/* enable period events when requested */
-
 
149
	if (period_event) {
-
 
150
		err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);
161
    err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);
151
		if (err < 0) {
162
    if (err < 0)
-
 
163
    {
152
			printf("Unable to set period event: %s\n", snd_strerror(err));
164
        printf("Unable to set period event: %s\n", snd_strerror(err));
153
			return err;
165
        return err;
154
		}
166
    }
155
	}
167
 
156
	/* write the parameters to the playback device */
168
    /* write the parameters to the playback device */
157
	err = snd_pcm_sw_params(handle, swparams);
169
    err = snd_pcm_sw_params(handle, swparams);
158
	if (err < 0) {
170
    if (err < 0)
-
 
171
    {
159
		printf("Unable to set sw params for playback: %s\n", snd_strerror(err));
172
        printf("Unable to set sw params for playback: %s\n", snd_strerror(err));
160
		return err;
173
        return err;
161
	}
174
    }
162
	return 0;
175
    return 0;
163
}
176
}
164
 
177
 
165
struct async_private_data {
178
struct async_private_data
-
 
179
{
166
	signed short *samples;
180
    signed short *samples;
167
	snd_pcm_channel_area_t *areas;
181
    snd_pcm_channel_area_t *areas;
168
	unsigned int period;
182
    unsigned int period;
169
};
183
};
170
 
184
 
171
 
185
 
172
////// SIGNAL GENERATION STUFF
186
////// SIGNAL GENERATION STUFF
173
/*int linear_chirp(int *pole, int delka_pole){  // vygeneruje linearni chirp a vzorky ulozi do pole
-
 
174
 
-
 
175
static const float f0 = 0.0001;
-
 
176
static const float k = 0.00001;
-
 
177
 
-
 
178
int t;
-
 
179
 
-
 
180
//  if((spozdeni+delka) < delka_pole)
-
 
181
    for(t=0;t < delka_pole;t++) pole[t] = round ( 10000*sin(2*M_PI*(t+faze)*(f0+(k/2)*(t+faze))) );
-
 
182
    faze +=t;
-
 
183
//  else return 0;
-
 
184
 
-
 
185
}*/
-
 
186
 
-
 
187
// vygeneruje linearni chirp a vzorky ulozi do pole
187
unsigned int linear_windowed_chirp(short *pole)
188
unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole,unsigned int offset)
-
 
189
{
188
{
190
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
189
    unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
191
 
190
 
192
static const float f0 = 1000;		//starting frequency
191
    static const float f0 = 1000;		//starting frequency
193
static const float fmax = 7000;		//ending frequency
192
    static const float fmax = 7000;		//ending frequency
194
static const float Tw = 0.002;
193
    static const float Tw = 0.002;
195
static float k;
194
    static float k;
196
 
195
 
197
unsigned int n=0;
196
    unsigned int n=0;
198
double t;
197
    double t;
199
unsigned int perioda;
198
    unsigned int chirp_samples;		// number of samples per period
200
 
199
 
201
  k=2*(fmax-f0)/Tw;
200
    k=2*(fmax-f0)/Tw;
202
  perioda = rate*Tw; 
201
    chirp_samples = ceil(rate*Tw);
203
 
202
 
204
  for(n=0;n<=perioda;n++){
203
    for (n=0;n<=chirp_samples;n++)
-
 
204
    {
205
     t = (double) n/ (double)rate;
205
        t = (double) n / (double)rate;
206
     pole[n+offset] = (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))) );
206
        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))) );
207
  }
207
    }
208
  return (perioda+offset);
208
    return (chirp_samples);
209
}
-
 
210
 
-
 
211
// generate sine samples and store
-
 
212
int sine(unsigned int *pole, unsigned int delka_pole)
-
 
213
{
-
 
214
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
-
 
215
unsigned int n;
-
 
216
double t;
-
 
217
 
-
 
218
  for(n=0;n < delka_pole;n++){
-
 
219
    t = 440.0 * (double) n/ (double)rate;
-
 
220
    pole[n] = (short) floor(maxval*sin(2*M_PI*t));
-
 
221
  }
-
 
222
}
-
 
223
//// generate simple sine ping
-
 
224
unsigned int sine_ping(unsigned int *pole, unsigned int delka_pole,unsigned int offset, double frequency)
-
 
225
{
-
 
226
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
-
 
227
unsigned int n;
-
 
228
double t;
-
 
229
 
-
 
230
  for(n=0;n < delka_pole;n++){
-
 
231
    t = frequency * (double) n/ (double)rate;
-
 
232
    pole[n] = (short) floor(maxval*sin(2*M_PI*t));
-
 
233
  }
-
 
234
}
209
}
235
 
210
 
236
/////////// CALL BACK STUFF ///////////////////
211
/////////// CALL BACK STUFF ///////////////////
237
static void async_playback_callback(snd_async_handler_t *ahandler)
212
static void async_playback_callback(snd_async_handler_t *ahandler)
238
{
213
{
239
	snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
214
    snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
240
	snd_pcm_sframes_t avail;
215
    snd_pcm_sframes_t avail;
241
	int err;
216
    int err;
242
	
217
 
243
	avail = snd_pcm_avail_update(handle);
218
    avail = snd_pcm_avail_update(handle);
244
	while ((avail >= period_size) && ((period*period_size) < chirp_size) ) {
219
    while ((avail >= period_size) && ((period*period_size) < chirp_size) )
-
 
220
    {
245
 
221
 
246
		err = snd_pcm_writei(handle, (chirp+period*period_size), period_size);
222
        err = snd_pcm_writei(handle, (chirp+period*period_size), period_size);
247
		if (err < 0) {
223
        if (err < 0)
-
 
224
        {
248
			printf("Write error: %s\n", snd_strerror(err));
225
            printf("Write error: %s\n", snd_strerror(err));
249
			exit(EXIT_FAILURE);
226
            exit(EXIT_FAILURE);
250
		}
227
        }
251
		if (err != period_size) {
228
        if (err != period_size)
-
 
229
        {
252
			printf("Write error: written %i expected %li\n", err, period_size);
230
            printf("Write error: written %i expected %li\n", err, period_size);
253
			exit(EXIT_FAILURE);
231
            exit(EXIT_FAILURE);
254
		}
232
        }
255
		avail = snd_pcm_avail_update(handle);
233
        avail = snd_pcm_avail_update(handle);
256
		period++;
234
        period++;
257
	}
235
    }
258
}
236
}
259
 
237
 
260
static void async_capture_callback(snd_async_handler_t *ahandler)
238
static void async_capture_callback(snd_async_handler_t *ahandler)
261
{
239
{
262
	snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
240
    snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
263
	snd_pcm_sframes_t avail;
241
    snd_pcm_sframes_t avail;
264
	int err;
242
    int err;
265
	
243
 
266
	avail = snd_pcm_avail_update(handle);
244
    avail = snd_pcm_avail_update(handle);
267
	while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) {  // segmentation fault checking disabled
245
    while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ )    // segmentation fault checking disabled
-
 
246
    {
268
 
247
 
269
		err = snd_pcm_readi(handle, (signal+cperiod*period_size), period_size);
248
        err = snd_pcm_readi(handle, (signal+cperiod*period_size), period_size);
270
		if (err < 0) {
249
        if (err < 0)
-
 
250
        {
271
			printf("Read error: %s\n", snd_strerror(err));
251
            printf("Read error: %s\n", snd_strerror(err));
272
			exit(EXIT_FAILURE);
252
            exit(EXIT_FAILURE);
273
		}
253
        }
274
		if (err != period_size) {
254
        if (err != period_size)
-
 
255
        {
275
			printf("Read error: red %i expected %li\n", err, period_size);
256
            printf("Read error: red %i expected %li\n", err, period_size);
276
			exit(EXIT_FAILURE);
257
            exit(EXIT_FAILURE);
277
		}
258
        }
278
		avail = snd_pcm_avail_update(handle);
259
        avail = snd_pcm_avail_update(handle);
279
		cperiod++;
260
        cperiod++;
280
	}
261
    }
281
}
262
}
282
 
263
 
283
 
264
 
284
int main(int argc, char *argv[])
265
int main(int argc, char *argv[])
285
{
266
{
286
	snd_pcm_t *playback_handle, *capture_handle;
267
    snd_pcm_t *playback_handle, *capture_handle;
287
	int err;
268
    int err;
288
	snd_pcm_hw_params_t *hwparams;
269
    snd_pcm_hw_params_t *hwparams;
289
	snd_pcm_sw_params_t *swparams;
270
    snd_pcm_sw_params_t *swparams;
290
	signed short *frame;  // pointer to array of samples
271
    signed short *frame;  // pointer to array of samples
291
	unsigned int chn;
272
    unsigned int chn;
292
	snd_pcm_channel_area_t *areas;
273
    snd_pcm_channel_area_t *areas;
293
 
274
 
294
	struct async_private_data data;
275
    struct async_private_data data;
295
	snd_async_handler_t *chandler, *phandler;
276
    snd_async_handler_t *chandler, *phandler;
296
	int count;
277
    int count;
297
	unsigned int i,j,m,n;
278
    unsigned int i,j,m,n;
298
	unsigned int delay[10];	//store delay of signifed correlation
279
    unsigned int delay[10];	//store delay of signifed correlation
299
  	long int l,r;  // store correlation at strict time
280
    long int l,r;  // store correlation at strict time
300
	long int correlationl[SIGNAL_SAMPLES]; //array to store correlation curve
281
    long int correlationl[SIGNAL_SAMPLES]; //array to store correlation curve
301
	long int correlationr[SIGNAL_SAMPLES]; //array to store correlation curve
282
    long int correlationr[SIGNAL_SAMPLES]; //array to store correlation curve
302
	int L_signal[SIGNAL_SAMPLES];
283
    int L_signal[SIGNAL_SAMPLES];
303
	int R_signal[SIGNAL_SAMPLES];
284
    int R_signal[SIGNAL_SAMPLES];
304
 
285
 
305
	FILE *out;
286
    FILE *out;
306
 
287
 
307
	snd_pcm_hw_params_alloca(&hwparams);
288
    snd_pcm_hw_params_alloca(&hwparams);
308
	snd_pcm_sw_params_alloca(&swparams);
289
    snd_pcm_sw_params_alloca(&swparams);
309
 
290
 
310
	printf("Simple PC sonar ver. 000000001 starting work.. \n");
291
    printf("Simple PC sonar ver. 000000001 starting work.. \n");
311
 
292
 
312
//open and set playback device
293
//open and set playback device
313
	if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
294
    if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)
-
 
295
    {
314
		printf("Playback open error: %s\n", snd_strerror(err));
296
        printf("Playback open error: %s\n", snd_strerror(err));
315
		return 0;
297
        return 0;
316
	}
298
    }
317
	
299
 
318
	if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0) {
300
    if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)
-
 
301
    {
319
		printf("Setting of hwparams failed: %s\n", snd_strerror(err));
302
        printf("Setting of hwparams failed: %s\n", snd_strerror(err));
320
		exit(EXIT_FAILURE);
303
        exit(EXIT_FAILURE);
321
	}
304
    }
322
	if ((err = set_swparams(playback_handle, swparams)) < 0) {
305
    if ((err = set_swparams(playback_handle, swparams)) < 0)
-
 
306
    {
323
		printf("Setting of swparams failed: %s\n", snd_strerror(err));
307
        printf("Setting of swparams failed: %s\n", snd_strerror(err));
324
		exit(EXIT_FAILURE);
308
        exit(EXIT_FAILURE);
325
	}
309
    }
326
 
310
 
327
//open and set capture device
311
//open and set capture device
328
	if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0) {
312
    if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)
-
 
313
    {
329
		printf("Playback open error: %s\n", snd_strerror(err));
314
        printf("Playback open error: %s\n", snd_strerror(err));
330
		return 0;
315
        return 0;
331
	}
316
    }
332
	
317
 
333
	if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0) {
318
    if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)
-
 
319
    {
334
		printf("Setting of hwparams failed: %s\n", snd_strerror(err));
320
        printf("Setting of hwparams failed: %s\n", snd_strerror(err));
335
		exit(EXIT_FAILURE);
321
        exit(EXIT_FAILURE);
336
	}
322
    }
337
	if ((err = set_swparams(capture_handle, swparams)) < 0) {
323
    if ((err = set_swparams(capture_handle, swparams)) < 0)
-
 
324
    {
338
		printf("Setting of swparams failed: %s\n", snd_strerror(err));
325
        printf("Setting of swparams failed: %s\n", snd_strerror(err));
339
		exit(EXIT_FAILURE);
326
        exit(EXIT_FAILURE);
340
	}
327
    }
341
 
328
 
342
// generate ping pattern
329
// generate ping pattern
-
 
330
    chirp = malloc(2*period_size * sizeof(short));
-
 
331
    chirp_size = linear_windowed_chirp(chirp);
343
 
332
 
344
        chirp_size=linear_windowed_chirp(chirp,1000000, CHIRP_OFFSET);
-
 
345
 
-
 
346
// register playback callback 
333
// register playback callback
347
	err = snd_async_add_pcm_handler(&phandler, playback_handle, async_playback_callback, &data); // fill by dummy &data
334
/*    err = snd_async_add_pcm_handler(&phandler, playback_handle, async_playback_callback, &data); // fill by dummy &data
348
	if (err < 0) {
335
    if (err < 0)
-
 
336
    {
349
		printf("Unable to register async handler\n");
337
        printf("Unable to register async handler\n");
350
		exit(EXIT_FAILURE);
338
        exit(EXIT_FAILURE);
351
	}
-
 
352
 
-
 
353
	if ((err = snd_pcm_prepare (playback_handle)) < 0) {
-
 
354
		fprintf (stderr, "cannot prepare audio interface for use (%s)\n",
-
 
355
			 snd_strerror (err));
-
 
356
		exit (1);
339
    }*/
357
	}
-
 
358
 
340
 
359
/*	for (period = 0; period < 2; period++) {
341
//    for (period = 0; period < 2; period++)
360
 
342
 
361
		err = snd_pcm_writei(playback_handle, (chirp+period*period_size), period_size);
343
    err = snd_pcm_writei(playback_handle, chirp, period_size);
362
		if (err < 0) {
344
    if (err < 0)
-
 
345
    {
363
			printf("Initial write error: %s\n", snd_strerror(err));
346
        printf("Initial write error: %s\n", snd_strerror(err));
364
			exit(EXIT_FAILURE);
347
        exit(EXIT_FAILURE);
365
		}
348
    }
366
		if (err != period_size) {
349
/*        if (err != period_size)
-
 
350
        {
367
			printf("Initial write error: written %i expected %li\n", err, period_size);
351
            printf("Initial write error: written %i expected %li\n", err, period_size);
368
			exit(EXIT_FAILURE);
352
            exit(EXIT_FAILURE);
369
		}
-
 
370
	}*/
353
        }*/
371
 
354
 
372
// register capture callback 
355
// register capture callback
373
	err = snd_async_add_pcm_handler(&chandler, capture_handle, async_capture_callback, &data); // fill by dummy &data
356
/*    err = snd_async_add_pcm_handler(&chandler, capture_handle, async_capture_callback, &data); // fill by dummy &data
374
	if (err < 0) {
357
    if (err < 0)
-
 
358
    {
375
		printf("Unable to register async handler\n");
359
        printf("Unable to register async handler\n");
376
		exit(EXIT_FAILURE);
360
        exit(EXIT_FAILURE);
377
	}
361
    }*/
378
 
362
 
379
        snd_pcm_link(capture_handle,playback_handle); //link capture and playback together
363
    snd_pcm_link(capture_handle,playback_handle); //link capture and playback together
380
 
364
 
381
//start sream
365
//start sream
382
	if ((err = snd_pcm_prepare (capture_handle)) < 0) {
366
/*    if ((err = snd_pcm_prepare (capture_handle)) < 0)
-
 
367
    {
383
		fprintf (stderr, "cannot prepare audio interface for use (%s)\n",
368
        fprintf (stderr, "cannot prepare audio interface for use (%s)\n",
384
			 snd_strerror (err));
369
                 snd_strerror (err));
385
		exit (1);
370
        exit (1);
386
	}
371
    }
-
 
372
    else printf("Capture device prepared...\n");*/
387
 
373
 
388
	err = snd_pcm_start(capture_handle);
374
    err = snd_pcm_start(playback_handle);
389
	if (err < 0) {
375
    if (err < 0)
-
 
376
    {
390
			printf("Start error: %s\n", snd_strerror(err));
377
        printf("Start error: %s\n", snd_strerror(err));
391
			exit(EXIT_FAILURE);
378
        exit(EXIT_FAILURE);
392
	}
379
    }
-
 
380
    else printf("Waiting for transmitt all samples\n");
393
	
381
 
394
//wait until all samples aren't transmitted
382
    while ( snd_pcm_avail(capture_handle) < period_size )
395
	printf("Waiting for transmitt all samples\n");
-
 
396
	while(cperiod<10) {
383
    {
397
		sleep(1);
384
        usleep(1000);
398
		printf(".");
385
        printf(".");
399
	}	
386
    }
400
 
387
 
-
 
388
    err = snd_pcm_drop(capture_handle);
401
////   stop audio??
389
    if (err < 0)
-
 
390
    {
-
 
391
        printf("Stop error: %s\n", snd_strerror(err));
-
 
392
        exit(EXIT_FAILURE);
-
 
393
    }
402
 
394
 
403
 
395
 
404
	j=0;
396
    j=0;
405
	for(i=0;i < SIGNAL_SAMPLES;i++){
397
    for (i=0;i < SIGNAL_SAMPLES;i++)
-
 
398
    {
406
	  L_signal[i]=signal[j];
399
        L_signal[i]=signal[j];
407
	  R_signal[i]=signal[j+1];
400
        R_signal[i]=signal[j+1];
408
	  j+=2;
401
        j+=2;
409
	}
402
    }
410
 
-
 
411
//        linear_windowed_chirp(L_signal,1000000, 1000);
-
 
412
 
403
 
413
	printf("\nData transmitted... \ncorrelating...\n");
404
    printf("Data transmitted... \ncorrelating...\n");
414
	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){
405
    for (n=0; n < (SIGNAL_SAMPLES - chirp_size);n++)
-
 
406
    {
415
	  l=0;
407
        l=0;
416
          r=0;
408
        r=0;
417
	  for(m=CHIRP_OFFSET;m < chirp_size;m++)
409
        for (m=0;m < chirp_size;m++)
418
          {
410
        {
419
            l += chirp[m]*L_signal[m+n];	// correlate with left channel
411
            l += chirp[m]*L_signal[m+n];	// correlate with left channel
420
            r += chirp[m]*R_signal[m+n];	// correlate with right channel
412
            r += chirp[m]*R_signal[m+n];	// correlate with right channel
421
          }
413
        }
422
	  correlationl[n]=l;
414
        correlationl[n]=l;
423
	  correlationr[n]=r;
415
        correlationr[n]=r;
424
	}
416
    }
425
 
417
 
426
	printf("\nSearching echos...\n");
418
    printf("Searching echos...\n");
427
	r=0;
419
    r=0;
428
	l=0;
420
    l=0;
429
	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){			//najde nejvetsi korelace
421
    for (n=0; n < (SIGNAL_SAMPLES - chirp_size);n++) 			//najde nejvetsi korelace
-
 
422
    {
430
	  if (l < correlationl[n]){
423
        if (l < correlationl[n])
-
 
424
        {
431
	  delay[1] = n;
425
            delay[1] = n;
432
	  l = correlationl[n];
426
            l = correlationl[n];
433
	  }
427
        }
434
	  if (r < correlationr[n]){
428
        if (r < correlationr[n])
-
 
429
        {
435
	  delay[2] = n;
430
            delay[2] = n;
436
	  r = correlationr[n];
431
            r = correlationr[n];
437
	  }
432
        }
438
	}
433
    }
439
 
434
 
-
 
435
    printf("\nWriting output file...\n");
440
  out=fopen("/tmp/sonar.txt","w");
436
    out=fopen("/tmp/sonar.txt","w");
441
  j=0;
437
    j=0;
442
  for(i=0;i<=100000;i++){
438
    for (i=0;i<=period_size;i++)
-
 
439
    {
443
    fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
440
        fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
444
    j+=2;
441
        j+=2;
445
  }
442
    }
446
  fclose(out);
443
    fclose(out);
447
 
444
 
448
	printf("\nEcho zacina na: %d vzorku.\n", delay[1]);
445
    printf("\nEcho zacina na: %d vzorku.\n", delay[1]);
449
	printf("Casove na: %f s\n", ((float)delay[1]/rate));
446
    printf("Casove na: %f s\n", ((float)delay[1]/rate));
450
	printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delay[1]/rate));
447
    printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delay[1]/rate));
451
 
448
 
452
	snd_pcm_close(playback_handle);
449
    snd_pcm_close(playback_handle);
453
	snd_pcm_close(capture_handle); 
450
    snd_pcm_close(capture_handle);
454
	return 0;
451
    return 0;
455
}
452
}
456
 
453