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