Subversion Repositories svnkaklik

Rev

Rev 543 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

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