Subversion Repositories svnkaklik

Rev

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

Rev 531 Rev 532
Line -... Line 1...
- 
 
 1 
///////////////////////////////////////////////////////////////////////////////////
- 
 
 2 
//                        This small demo of sonar.
- 
 
 3 
// Program allow distance measuring.
1 
/*
 4 
//
2 
 *  This small demo sends a simple sinusoidal wave to your speakers.
 - 
 
3 
 */
 5 
//
- 
 
 6 
///////////////////////////////////////////////////////////////////////////////////
4 
 
 7 
 
5 
#include <stdio.h>
 8 
#include <stdio.h>
6 
#include <stdlib.h>
 9 
#include <stdlib.h>
7 
#include <string.h>
 10 
#include <string.h>
8 
#include <sched.h>
 11 
#include <sched.h>
Line 25... Line 28...
25 
 
 28 
 
26 
unsigned int chirp_size;
 29 
unsigned int chirp_size;
27 
 
 30 
 
28 
int period=0;
 31 
int period=0;
29 
int cperiod=0;
 32 
int cperiod=0;
30 
int chirp[1000000];
 33 
int chirp[100000];
31 
short signal[44100*6];		// record 6s of input samples
 34 
short signal[1000000];		// record 6s of input samples
32 
 
 35 
 
33 
static snd_pcm_sframes_t buffer_size;	// size of buffer at sound card
 36 
static snd_pcm_sframes_t buffer_size;	// size of buffer at sound card
34 
static snd_pcm_sframes_t period_size;	//samples per frame
 37 
static snd_pcm_sframes_t period_size;	//samples per frame
35 
static snd_output_t *output = NULL;
 38 
static snd_output_t *output = NULL;
36 
 
 39 
 
37 
/*static void generate_sine(const snd_pcm_channel_area_t *areas,
- 
 
38 
			  snd_pcm_uframes_t offset,
 - 
 
39 
			  int count, double *_phase)
 - 
 
40 
{
 - 
 
41 
	static double max_phase = 2. * M_PI;
 - 
 
42 
	double phase = *_phase;
 - 
 
43 
	double step = max_phase*freq/(double)rate;
 - 
 
44 
	double res;
 - 
 
45 
	unsigned char *samples[channels], *tmp;
 - 
 
46 
	int steps[channels];
 - 
 
47 
	unsigned int chn, byte;
 - 
 
48 
	union {
 - 
 
49 
		int i;
 - 
 
50 
		unsigned char c[4];
 - 
 
51 
	} ires;
 - 
 
52 
	unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
 - 
 
53 
	int bps = snd_pcm_format_width(format) / 8;  /* bytes per sample */
 - 
 
54
- 
 
55 
	/* verify and prepare the contents of areas
- 
 
56 
	for (chn = 0; chn < channels; chn++) {
 - 
 
57 
		if ((areas[chn].first % 8) != 0) {
 - 
 
58 
			printf("areas[%i].first == %i, aborting...\n", chn, areas[chn].first);
 - 
 
59 
			exit(EXIT_FAILURE);
 - 
 
60 
		}
 - 
 
61 
		samples[chn] = /*(signed short *)(((unsigned char *)areas[chn].addr) + (areas[chn].first / 8));
 - 
 
62 
		if ((areas[chn].step % 16) != 0) {
 - 
 
63 
			printf("areas[%i].step == %i, aborting...\n", chn, areas[chn].step);
 - 
 
64 
			exit(EXIT_FAILURE);
 - 
 
65 
		}
 - 
 
66 
		steps[chn] = areas[chn].step / 8;
 - 
 
67 
		samples[chn] += offset * steps[chn];
 - 
 
68 
	}
 - 
 
69 
	/* fill the channel areas
- 
 
70 
	while (count-- > 0) {
 - 
 
71 
		res = sin(phase) * maxval;
 - 
 
72 
		ires.i = res;
 - 
 
73 
		tmp = ires.c;
 - 
 
74 
		for (chn = 0; chn < channels; chn++) {
 - 
 
75 
			for (byte = 0; byte < (unsigned int)bps; byte++)
 - 
 
76 
				*(samples[chn] + byte) = tmp[byte];
 - 
 
77 
			samples[chn] += steps[chn];
 - 
 
78 
		}
 - 
 
79 
		phase += step;
 - 
 
80 
		if (phase >= max_phase)
 - 
 
81 
			phase -= max_phase;
 - 
 
82 
	}
 - 
 
83 
	*_phase = phase;
 - 
 
84 
}*/
 - 
 
85 
 
 - 
 
86 
static int set_hwparams(snd_pcm_t *handle,
 40 
static int set_hwparams(snd_pcm_t *handle,
87 
			snd_pcm_hw_params_t *params,
 41 
			snd_pcm_hw_params_t *params,
88 
			snd_pcm_access_t access)
 42 
			snd_pcm_access_t access)
89 
{
 43 
{
90 
	unsigned int rrate;
 44 
	unsigned int rrate;
Line 224... Line 178...
224 
    faze +=t;
 178 
    faze +=t;
225 
//  else return 0;
 179 
//  else return 0;
226 
 
 180 
 
227 
}*/
 181 
}*/
228 
 
 182 
 
229 
 
 - 
 
230 
 
 - 
 
- 
 
 183 
// vygeneruje linearni chirp a vzorky ulozi do pole
231 
unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole){  // vygeneruje linearni chirp a vzorky ulozi do pole
 184 
unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole)
232 
 
 185 
{
233 
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
 186 
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
234 
 
 187 
 
235 
static const float f0 = 1000;
 188 
static const float f0 = 1000;
236 
static const float fmax = 5000;
 189 
static const float fmax = 5000;
237 
static const float Tw = 0.2;
 190 
static const float Tw = 0.2;
Line 239... Line 192...
239 
 
 192 
 
240 
unsigned int n=0;
 193 
unsigned int n=0;
241 
double t;
 194 
double t;
242 
unsigned int perioda;
 195 
unsigned int perioda;
243 
 
 196 
 
244 
k=2*(fmax-f0)/Tw;
 197 
  k=2*(fmax-f0)/Tw;
245 
perioda = rate*Tw;
198 
  perioda = rate*Tw;
246 
 
 199 
 
247 
   for(n=0;n<=perioda;n++){
 200 
  for(n=0;n<=perioda;n++){
248 
      t = (double) n/ (double)rate;
 201 
     t = (double) n/ (double)rate;
249 
      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))) );
 202 
     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))) );
250 
   }
 203 
  }
251 
   return perioda;
 204 
  return perioda;
252 
}
 205 
}
253 
 
 206 
 
- 
 
 207 
// generate sine samples and store
254 
int sine(unsigned int *pole, unsigned int delka_pole)
 208 
int sine(unsigned int *pole, unsigned int delka_pole)
255 
{
 209 
{
256 
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
 210 
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
257 
unsigned int n;
 211 
unsigned int n;
258 
double t;
 212 
double t;
Line 299... Line 253...
299 
	snd_pcm_channel_area_t *areas = data->areas;*/
 253 
	snd_pcm_channel_area_t *areas = data->areas;*/
300 
	snd_pcm_sframes_t avail;
 254 
	snd_pcm_sframes_t avail;
301 
	int err;
 255 
	int err;
302
256
303 
	avail = snd_pcm_avail_update(handle);
 257 
	avail = snd_pcm_avail_update(handle);
304 
	while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) {
 258 
	while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) {  // segmentation fault checking disabled
305 
 
 259 
 
306 
		err = snd_pcm_readi(handle, (signal+cperiod*period_size), period_size);
 260 
		err = snd_pcm_readi(handle, (signal+cperiod*period_size), period_size);
307 
		if (err < 0) {
 261 
		if (err < 0) {
308 
			printf("Read error: %s\n", snd_strerror(err));
 262 
			printf("Read error: %s\n", snd_strerror(err));
309 
			exit(EXIT_FAILURE);
 263 
			exit(EXIT_FAILURE);
Line 329... Line 283...
329 
	snd_pcm_channel_area_t *areas;
 283 
	snd_pcm_channel_area_t *areas;
330 
 
 284 
 
331 
	struct async_private_data data;
 285 
	struct async_private_data data;
332 
	snd_async_handler_t *chandler, *phandler;
 286 
	snd_async_handler_t *chandler, *phandler;
333 
	int count;
 287 
	int count;
334 
	unsigned int i,j;
 288 
	unsigned int i,j,m,n;
- 
 
 289 
	unsigned int delay;	//store delay of maximal correlation
- 
 
 290 
  	double r;
- 
 
 291 
	double correlation[1000000]; //array to store correlation curve
335 
 
 292 
 
336 
	FILE *out;
 293 
	FILE *out;
337 
 
 294 
 
338 
 
 - 
 
339 
	snd_pcm_hw_params_alloca(&hwparams);
 295 
	snd_pcm_hw_params_alloca(&hwparams);
340 
	snd_pcm_sw_params_alloca(&swparams);
 296 
	snd_pcm_sw_params_alloca(&swparams);
341 
 
 297 
 
342 
//open adn set playback device
 298 
//open and set playback device
343 
	if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
 299 
	if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
344 
		printf("Playback open error: %s\n", snd_strerror(err));
 300 
		printf("Playback open error: %s\n", snd_strerror(err));
345 
		return 0;
 301 
		return 0;
346 
	}
 302 
	}
347
303
Line 417... Line 373...
417 
	if (err < 0) {
 373 
	if (err < 0) {
418 
			printf("Start error: %s\n", snd_strerror(err));
 374 
			printf("Start error: %s\n", snd_strerror(err));
419 
			exit(EXIT_FAILURE);
 375 
			exit(EXIT_FAILURE);
420 
	}
 376 
	}
421
377
422 
 
 - 
 
423 
 
 - 
 
424 
	/* because all other work is done in the signal handler,
 378 
//wait until all samples aren't transmitted
425 
	   suspend the process */
 379 
	printf("Waiting for transmitt all samples\n");
426 
	while(cperiod<10) {
 380 
	while(cperiod<10) {
427 
		sleep(1);
 381 
		sleep(1);
- 
 
 382 
		printf(".");
428 
	}
383 
	}
- 
 
 384 
	printf("\nData transmitted... \nprocessing...\n");
429 
 
 385 
 
430 
	out=fopen("./output.txt","w");
 386 
	out=fopen("./output.txt","w");
431 
	for(i=0;i<=100000;i++) fprintf(out,"%6d	%6d \n",chirp[i],signal[i]);
387 
	for(i=0;i<=100000;i++) fprintf(out,"%6d	%6d %6d \n",i,chirp[i],signal[i]);
432 
	fclose(out);
 388 
	fclose(out);
433 
 
 389 
 
- 
 
 390 
#define SIGNAL_SAMPLES 100000
- 
 
 391 
#define SAMPLES  1000
- 
 
 392 
 
- 
 
 393 
   for(n=0; n < (SIGNAL_SAMPLES - SAMPLES);n++){			//spocita korelaci pro mozna spozdeni
- 
 
 394 
     r=0;
- 
 
 395 
     for(m=0;m < SAMPLES;m++) r += chirp[m]*signal[m+n];
- 
 
 396 
     correlation[n]=r;
- 
 
 397 
   }
- 
 
 398 
 
- 
 
 399 
   r=0;
- 
 
 400 
   for(n=0; n < (SIGNAL_SAMPLES - SAMPLES);n++){			//najde nejvetsi shodu (pro nazornost v samostatnem cyklu)
- 
 
 401 
     if (r < correlation[n]){
- 
 
 402 
       delay = n;
- 
 
 403 
       r = correlation[n];
- 
 
 404 
     }
- 
 
 405 
   }
- 
 
 406 
 
- 
 
 407 
	printf("\nEcho zacina na: %d vzorku.\n", delay);
- 
 
 408 
	printf("\nCasove na: %ds", delay);
- 
 
 409 
 
434 
	snd_pcm_close(playback_handle);
 410 
	snd_pcm_close(playback_handle);
435 
	snd_pcm_close(capture_handle);
 411 
	snd_pcm_close(capture_handle);
436 
	return 0;
 412 
	return 0;
437 
}
 413 
}
438 
 
 414