Subversion Repositories svnkaklik

Rev

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

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