WebSVN – svnkaklik – Diff – /programy/C/ix86/sound/sonar.c


///////////////////////////////////////////////////////////////////////////////////
//                        A small demo of sonar.
// Program allow distance measuring.
// Uses cross-correlation algorithm to find echos
//
// Author: kaklik  (kaklik@mlab.cz)
//
///////////////////////////////////////////////////////////////////////////////////
 
#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>
 
static char *device = "plughw:0,0";			/* playback device */
static snd_pcm_format_t format = SND_PCM_FORMAT_S16;	/* sample format */
static unsigned int rate = 98000;			/* stream rate */
static unsigned int buffer_time = 500000;		/* ring buffer length in us */
static unsigned int period_time = 100000;		/* period time in us */
static int verbose = 0;					/* verbose flag */
static int resample = 1;				/* enable alsa-lib resampling */
static int period_event = 0;				/* produce poll event after each period */
 
#define SOUND_SPEED	340
#define SIGNAL_SAMPLES 100000
#define CHIRP_OFFSET	0 
 
unsigned int chirp_size;
 
int period=0;
int cperiod=0;
int chirp[100000];
short signal[1000000];		// record 6s of input samples
 
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;
	/* 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;
	/* 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: */
	/* (buffer_size / avail_min) * avail_min */
	err = snd_pcm_sw_params_set_start_threshold(handle, swparams, (buffer_size / period_size) * period_size);
	if (err < 0) {
		printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));
		return err;
	}
	/* allow the transfer when at least period_size samples can be processed */
	/* or disable this mechanism when period event is enabled (aka interrupt like style processing) */
	err = snd_pcm_sw_params_set_avail_min(handle, swparams, period_event ? buffer_size : period_size);
	if (err < 0) {
		printf("Unable to set avail min for playback: %s\n", snd_strerror(err));
		return err;
	}
	/* enable period events when requested */
	if (period_event) {
		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;
}
 
struct async_private_data {
	signed short *samples;
	snd_pcm_channel_area_t *areas;
	unsigned int period;
};
 
 
////// SIGNAL GENERATION STUFF
/*int linear_chirp(int *pole, int delka_pole){  // vygeneruje linearni chirp a vzorky ulozi do pole
 
static const float f0 = 0.0001;
static const float k = 0.00001;
 
int t;
 
//  if((spozdeni+delka) < delka_pole)
    for(t=0;t < delka_pole;t++) pole[t] = round ( 10000*sin(2*M_PI*(t+faze)*(f0+(k/2)*(t+faze))) );
    faze +=t;
//  else return 0;
 
}*/
 
// vygeneruje linearni chirp a vzorky ulozi do pole
unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole,unsigned int offset)
{
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
 
static const float f0 = 1000;
static const float fmax = 7000;
static const float Tw = 0.002;
static float k;
 
unsigned int n=0;
double t;
unsigned int perioda;
 
  k=2*(fmax-f0)/Tw;
  perioda = rate*Tw; 
 
  for(n=0;n<=perioda;n++){
     t = (double) n/ (double)rate;
     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))) );
  }
  return (perioda+offset);
}
 
// generate sine samples and store
int sine(unsigned int *pole, unsigned int delka_pole)
{
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
unsigned int n;
double t;
 
  for(n=0;n < delka_pole;n++){
    t = 440.0 * (double) n/ (double)rate;
    pole[n] = (short) floor(maxval*sin(2*M_PI*t));
  }
}
//// generate simple sine ping
unsigned int sine_ping(unsigned int *pole, unsigned int delka_pole,unsigned int offset, double frequency)
{
unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
unsigned int n;
double t;
 
  for(n=0;n < delka_pole;n++){
    t = frequency * (double) n/ (double)rate;
    pole[n] = (short) floor(maxval*sin(2*M_PI*t));
  }
}
 
/////////// CALL BACK STUFF ///////////////////
static void async_playback_callback(snd_async_handler_t *ahandler)
{
	snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
/*	struct async_private_data *data = snd_async_handler_get_callback_private(ahandler);
	signed short *samples = data->samples;
	snd_pcm_channel_area_t *areas = data->areas;*/
	snd_pcm_sframes_t avail;
	int err;
	
	avail = snd_pcm_avail_update(handle);
	while ((avail >= period_size) && ((period*period_size) < chirp_size) ) {
 
		err = snd_pcm_writei(handle, (chirp+period*period_size), period_size);
		if (err < 0) {
			printf("Write error: %s\n", snd_strerror(err));
			exit(EXIT_FAILURE);
		}
		if (err != period_size) {
			printf("Write error: written %i expected %li\n", err, period_size);
			exit(EXIT_FAILURE);
		}
		avail = snd_pcm_avail_update(handle);
		period++;
	}
}
 
static void async_capture_callback(snd_async_handler_t *ahandler)
{
	snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
/*	struct async_private_data *data = snd_async_handler_get_callback_private(ahandler);
	signed short *samples = data->samples;
	snd_pcm_channel_area_t *areas = data->areas;*/
	snd_pcm_sframes_t avail;
	int err;
	
	avail = snd_pcm_avail_update(handle);
	while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) {  // segmentation fault checking disabled
 
		err = snd_pcm_readi(handle, (signal+cperiod*period_size), period_size);
		if (err < 0) {
			printf("Read error: %s\n", snd_strerror(err));
			exit(EXIT_FAILURE);
		}
		if (err != period_size) {
			printf("Read error: red %i expected %li\n", err, period_size);
			exit(EXIT_FAILURE);
		}
		avail = snd_pcm_avail_update(handle);
		cperiod++;
	}
}
 
 
int main(int argc, char *argv[])
{
	snd_pcm_t *playback_handle, *capture_handle;
	int err;
	snd_pcm_hw_params_t *hwparams;
	snd_pcm_sw_params_t *swparams;
	signed short *frame;  // pointer to array of samples
	unsigned int chn;
	snd_pcm_channel_area_t *areas;
 
	struct async_private_data data;
	snd_async_handler_t *chandler, *phandler;
	int count;
	unsigned int i,j,m,n;
	unsigned int delay[10];	//store delay of signifed correlation
  	long int l,r;  // store correlation at strict time
	long int correlationl[SIGNAL_SAMPLES]; //array to store correlation curve
	long int correlationr[SIGNAL_SAMPLES]; //array to store correlation curve
	int L_signal[SIGNAL_SAMPLES];
	int R_signal[SIGNAL_SAMPLES];
 
	FILE *out;
 
	snd_pcm_hw_params_alloca(&hwparams);
	snd_pcm_sw_params_alloca(&swparams);
 
	printf("Simple PC sonar ver. 000000001 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);
	}
 
/// generate ping pattern
 
        chirp_size=linear_windowed_chirp(chirp,1000000, CHIRP_OFFSET);
 
/// register playback callback 
	err = snd_async_add_pcm_handler(&phandler, playback_handle, async_playback_callback, &data); // fill by dummy &data
	if (err < 0) {
		printf("Unable to register async handler\n");
		exit(EXIT_FAILURE);
	}
	for (period = 0; period < 2; period++) {
 
		err = snd_pcm_writei(playback_handle, (chirp+period*period_size), period_size);
		if (err < 0) {
			printf("Initial write error: %s\n", snd_strerror(err));
			exit(EXIT_FAILURE);
		}
		if (err != period_size) {
			printf("Initial write error: written %i expected %li\n", err, period_size);
			exit(EXIT_FAILURE);
		}
	}
 
// register capture callback 
	err = snd_async_add_pcm_handler(&chandler, capture_handle, async_capture_callback, &data); // fill by dummy &data
	if (err < 0) {
		printf("Unable to register async handler\n");
		exit(EXIT_FAILURE);
	}
 
        snd_pcm_link(capture_handle,playback_handle); //link capture and playback together
 
//start sream
	if ((err = snd_pcm_prepare (capture_handle)) < 0) {
		fprintf (stderr, "cannot prepare audio interface for use (%s)\n",
			 snd_strerror (err));
		exit (1);
	}
 
	err = snd_pcm_start(capture_handle);
	if (err < 0) {
			printf("Start error: %s\n", snd_strerror(err));
			exit(EXIT_FAILURE);
	}
	
 
 
 
 
 
 
 
 
 
 
 
//wait until all samples aren't transmitted
	printf("Waiting for transmitt all samples\n");
	while(cperiod<10) {
		sleep(1);
		printf(".");
	}	
 
////   stop audio??
 
 
	j=0;
	for(i=0;i < SIGNAL_SAMPLES;i++){
	  L_signal[i]=signal[j];
	  R_signal[i]=signal[j+1];
	  j+=2;
	}
 
//        linear_windowed_chirp(L_signal,1000000, 1000);
 
	printf("\nData transmitted... \ncorrelating...\n");
	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){
	  l=0;
          r=0;
	  for(m=CHIRP_OFFSET;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]=l;
	  correlationr[n]=r;
	}
 
	printf("\nSearching echos...\n");
	r=0;
	l=0;
	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){			//najde nejvetsi korelace
	  if (l < correlationl[n]){
	  delay[1] = n;
	  l = correlationl[n];
	  }
	  if (r < correlationr[n]){
	  delay[2] = n;
	  r = correlationr[n];
	  }
	}
 
  out=fopen("./output.txt","w");
  j=0;
  for(i=0;i<=100000;i++){
    fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
    j+=2;
  }
  fclose(out);
 
	printf("\nEcho zacina na: %d vzorku.\n", delay[1]);
	printf("Casove na: %f s\n", ((float)delay[1]/rate));
	printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delay[1]/rate));
 
	snd_pcm_close(playback_handle);
	snd_pcm_close(capture_handle); 
	return 0;
}
 

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

Subversion Repositories svnkaklik

(root)/programy/C/ix86/sound/sonar.c – Rev 545 → 548