Rev Author Line No. Line
3297 kaklik 1 ///////////////////////////////////////////////////////////////////////////////////
2 // A small demo of sonar.
3 // Program allow distance measuring.
4 // Uses cross-correlation algorithm to find echos
5 //
6 // Author: kaklik (kaklik@mlab.cz)
7 //$Id:$
8 ///////////////////////////////////////////////////////////////////////////////////
9  
10 #include <stdio.h>
11 #include <stdlib.h>
12 #include <string.h>
13 #include <sched.h>
14 #include <errno.h>
15 #include <getopt.h>
16 #include <alsa/asoundlib.h>
17 #include <sys/time.h>
18 #include <math.h>
19 #include <fftw3.h>
20  
21 #define SOUND_SPEED 340.0 // sound speed in air in metrs per second
22 #define MAX_RANGE 5.0 // maximal working radius in meters
23 #define Xl -0.1 // microphones position
24 #define Xr 0.1
25  
26 static char *device = "plughw:0,0"; /* playback device */
27 static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */
28 static unsigned int rate = 96000; /* stream rate */
29 static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */
30 static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */
31 static int resample = 1; /* enable alsa-lib resampling */
32  
33 unsigned int chirp_size;
34  
35 static snd_pcm_sframes_t buffer_size; // size of buffer at sound card
36 static snd_pcm_sframes_t period_size; //samples per frame
37 static snd_output_t *output = NULL;
38  
39 static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels)
40 {
41 unsigned int rrate;
42 snd_pcm_uframes_t size;
43 int err, dir;
44  
45 /* choose all parameters */
46 err = snd_pcm_hw_params_any(handle, params);
47 if (err < 0)
48 {
49 printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));
50 return err;
51 }
52 /* set hardware resampling */
53 err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);
54 if (err < 0)
55 {
56 printf("Resampling setup failed for playback: %s\n", snd_strerror(err));
57 return err;
58 }
59 /* set the interleaved read/write format */
60 err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);
61 if (err < 0)
62 {
63 printf("Access type not available for playback: %s\n", snd_strerror(err));
64 return err;
65 }
66 /* set the sample format */
67 err = snd_pcm_hw_params_set_format(handle, params, format);
68 if (err < 0)
69 {
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 {
77 printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));
78 return err;
79 }
80 /* set the stream rate */
81 rrate = rate;
82 err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);
83 if (err < 0)
84 {
85 printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));
86 return err;
87 }
88 if (rrate != rate)
89 {
90 printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);
91 return -EINVAL;
92 }
93 else printf("Rate set to %i Hz\n", rate, err);
94 /* set the buffer time */
95 err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);
96 if (err < 0)
97 {
98 printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));
99 return err;
100 }
101 err = snd_pcm_hw_params_get_buffer_size(params, &size);
102 if (err < 0)
103 {
104 printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));
105 return err;
106 }
107 buffer_size = size;
108 printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);
109  
110  
111 // set the period time
112 err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);
113 if (err < 0)
114 {
115 printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));
116 return err;
117 }
118  
119 err = snd_pcm_hw_params_get_period_size(params, &size, &dir);
120 if (err < 0)
121 {
122 printf("Unable to get period size for playback: %s\n", snd_strerror(err));
123 return err;
124 }
125 period_size = size;
126 printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);
127  
128 /* write the parameters to device */
129 err = snd_pcm_hw_params(handle, params);
130 if (err < 0)
131 {
132 printf("Unable to set hw params for playback: %s\n", snd_strerror(err));
133 return err;
134 }
135 return 0;
136 }
137  
138 static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams)
139 {
140 int err;
141  
142 /* get the current swparams */
143 err = snd_pcm_sw_params_current(handle, swparams);
144 if (err < 0)
145 {
146 printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err));
147 return err;
148 }
149 // start the transfer when the buffer is almost full: never fou our case
150 err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);
151 if (err < 0)
152 {
153 printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));
154 return err;
155 }
156  
157 err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);
158 if (err < 0)
159 {
160 printf("Unable to set period event: %s\n", snd_strerror(err));
161 return err;
162 }
163  
164 /* write the parameters to the playback device */
165 err = snd_pcm_sw_params(handle, swparams);
166 if (err < 0)
167 {
168 printf("Unable to set sw params for playback: %s\n", snd_strerror(err));
169 return err;
170 }
171 return 0;
172 }
173  
174 ////// SIGNAL GENERATION STUFF
175 unsigned int linear_windowed_chirp(short *pole) // generate the ping signal
176 {
177 unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
178  
179 static const float f0 = 5000; //starting frequency
180 static const float fmax = 10000; //ending frequency
181 static const float Tw = 0.0015; // time width of ping in seconds
182 static float k;
183  
184 unsigned int n=0;
185 double t;
186 unsigned int chirp_samples; // number of samples per period
187  
188 k=2*(fmax-f0)/Tw;
189 chirp_samples = ceil(rate*Tw); // compute size of ping sinal in samples
190  
191 for (n=0;n<=chirp_samples;n++)
192 {
193 t = (double) n / (double)rate;
194 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))) ); // signal generation formula
195 }
196 return (chirp_samples); // return count of samples in ping
197 }
198  
199 int main(int argc, char *argv[])
200 {
201 snd_pcm_t *playback_handle, *capture_handle;
202 int err;
203 snd_pcm_hw_params_t *hwparams;
204 snd_pcm_sw_params_t *swparams;
205  
206 long int *correlationl, *correlationr;
207 float *echo_map;
208 int *L_signal, *R_signal;
209 short *chirp, *signal;
210 float *chirp_spect, *lecho_spect, *recho_spect;
211 float a,b; // sides of trilateration triangle.
212 float f,g; //measured lenght path of signal
213 unsigned int i,j,m,n;
214 unsigned int map_size; //number of points in echo map.
215 unsigned int delayl[10],delayr[10]; //store delay of signifed correlation
216 long int l,r; // store correlation at strict time
217 double df; //frequency resolution
218 double k; // sample numbers to distance normalising constant
219 unsigned int frequency_bins; // number of output frequency bins
220  
221 double *inchirp; // Fourier transform variables
222 fftw_complex *outchirp;
223 fftw_plan fft_plan_chirp;
224  
225 FILE *out; // dummy variable for file data output
226  
227 snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registers
228 snd_pcm_sw_params_alloca(&swparams);
229  
230 printf("Simple PC sonar $Rev:$ starting work.. \n");
231  
232 //open and set playback device
233 if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)
234 {
235 printf("Playback open error: %s\n", snd_strerror(err));
236 return 0;
237 }
238  
239 if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0)
240 {
241 printf("Setting of hwparams failed: %s\n", snd_strerror(err));
242 exit(EXIT_FAILURE);
243 }
244 if ((err = set_swparams(playback_handle, swparams)) < 0)
245 {
246 printf("Setting of swparams failed: %s\n", snd_strerror(err));
247 exit(EXIT_FAILURE);
248 }
249  
250 //open and set capture device
251 if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0)
252 {
253 printf("Playback open error: %s\n", snd_strerror(err));
254 return 0;
255 }
256  
257 if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0)
258 {
259 printf("Setting of hwparams failed: %s\n", snd_strerror(err));
260 exit(EXIT_FAILURE);
261 }
262 if ((err = set_swparams(capture_handle, swparams)) < 0)
263 {
264 printf("Setting of swparams failed: %s\n", snd_strerror(err));
265 exit(EXIT_FAILURE);
266 }
267  
268 /* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together
269 if (err < 0)
270 {
271 printf("Device linking error: %s\n", snd_strerror(err));
272 exit(EXIT_FAILURE);
273 }*/
274  
275 k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance
276  
277 correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve
278 correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve
279 L_signal = malloc(period_size * sizeof(int));
280 R_signal = malloc(period_size * sizeof(int));
281 chirp = calloc(2*period_size, sizeof(short));
282 signal = malloc(2*period_size * sizeof(short));
283  
284 map_size=0;
285 for (i=0;i < period_size; i++) // brute force function for compute number of points in echo map.
286 {
287 a=k*i;
288 for(j=0;j < period_size; j++)
289 {
290 b=k*j;
291 if( (Xl <= a) && (Xr <= b) ) map_size++;
292 }
293 }
294 echo_map = malloc((3*map_size) * sizeof(float)); // Array to store 2D image of echos
295 if (echo_map == NULL) printf("Can't allocate enought memory");
296  
297 // generate ping pattern
298 chirp_size = linear_windowed_chirp(chirp);
299  
300 frequency_bins = chirp_size / 2 + 1;
301 df = (double) rate / (double) chirp_size;
302 chirp_spect = malloc(frequency_bins * sizeof(float));
303 lecho_spect = malloc(frequency_bins * sizeof(float));
304 recho_spect = malloc(frequency_bins * sizeof(float));
305  
306 inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT
307 outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);
308  
309 fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);
310  
311 printf("compute chirp spectrum\n");
312 for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i];
313 fftw_execute(fft_plan_chirp);
314 for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] );
315  
316 // write chirp data to souncard buffer
317 err = snd_pcm_writei(playback_handle, chirp, period_size);
318 if (err < 0)
319 {
320 printf("Initial write error: %s\n", snd_strerror(err));
321 exit(EXIT_FAILURE);
322 }
323  
324 //start sream
325 err = snd_pcm_start(playback_handle);
326 if (err < 0)
327 {
328 printf("Start error: %s\n", snd_strerror(err));
329 exit(EXIT_FAILURE);
330 }
331  
332 err = snd_pcm_start(capture_handle);
333 if (err < 0)
334 {
335 printf("Start error: %s\n", snd_strerror(err));
336 exit(EXIT_FAILURE);
337 }
338 else printf("Transmitting all samples of chirp\n");
339 //--------------
340  
341 while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data
342 {
343 usleep(1000);
344 printf(".");
345 }
346  
347 err = snd_pcm_drop(playback_handle); // stop audio stream
348 err = snd_pcm_drain(capture_handle);
349 if (err < 0)
350 {
351 printf("Stop error: %s\n", snd_strerror(err));
352 exit(EXIT_FAILURE);
353 }
354  
355 err = snd_pcm_readi(capture_handle, signal, period_size); //read period from audio buffer
356 if (err < 0)
357 {
358 printf("Read error: %s\n", snd_strerror(err));
359 exit(EXIT_FAILURE);
360 }
361  
362 j=0;
363 for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays
364 {
365 L_signal[i]=signal[j];
366 R_signal[i]=signal[j+1];
367 j+=2;
368 }
369  
370 printf("\nChirp transmitted \ncorrelating\n");
371 for (n=0; n < (period_size - chirp_size - 1); n++)
372 {
373 l=0;
374 r=0;
375 for ( m = 0; m < chirp_size;m++)
376 {
377 l += chirp[m]*L_signal[m+n]; // correlate with left channel
378 r += chirp[m]*R_signal[m+n]; // correlate with right channel
379 }
380 correlationl[n]=abs(l);
381 correlationr[n]=abs(r);
382 }
383  
384 m=0;
385 printf("Building echo map\n"); // compute map from left and right correlation data
386 for (i=0; i < period_size; i++)
387 {
388 f=k*i; // transform number of sample to distance (divide by 2 becouse path of signal is aproximmately 2times longer than distance)
389 for(j=0; j < period_size; j++)
390 {
391 g=k*j;
392  
393 a=(2*f*g*Xl+f*f*Xr+Xl*(g*g+(Xl-Xr)*Xr))/(2*g*Xl+2*f*Xr);
394 b=(g*g*Xl-2*f*g*Xr+Xr*(f*f+Xl*(-Xl+Xr)))/(2*g*Xl-2*f*Xr);
395  
396 if( ((Xr-Xl) <= a+b) && (b <= a+(Xr-Xl)) && (a <= b+(Xr-Xl)) ) // kontrola trojuhelnikove nerovnosti
397 {
398 printf("%f %f\n",a,b);
399 echo_map[m]=(f*((f-g)*g + Xl*Xl)-g*Xr*Xr)/(2*f*Xl-2*g*Xr);
400 echo_map[m+1]=sqrt( ((-g*g+Xl*Xl)*(f-Xr)*(f-g+Xl-Xr)*(f+Xr)*(f-g-Xl+Xr))/(4*(f*Xl-g*Xr)*(f*Xl-g*Xr)) );
401 echo_map[m+2]=(correlationl[i]+correlationr[j])/2;
402 m+=3;
403 }
404 }
405 }
406  
407 printf("Searching echos\n");
408 r=0;
409 l=0;
410 for (n=0; n < period_size;n++) //najde nejvetsi korelace
411 {
412 if (l < correlationl[n])
413 {
414 delayl[1] = n;
415 l = correlationl[n];
416 }
417 if (r < correlationr[n])
418 {
419 delayr[1] = n;
420 r = correlationr[n];
421 }
422 }
423  
424 //spocitej frekvencni spektrum pro levy kanal
425 for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];
426 fftw_execute(fft_plan_chirp);
427 for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
428  
429  
430 // napln pole daty z praveho kanalu a spocitej frekvencni spektrum
431 for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];
432 fftw_execute(fft_plan_chirp);
433 for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
434  
435 printf("Writing output files\n");
436 out=fopen("/tmp/sonar.txt","w");
437 for (i=0; i <= (period_size - 1); i++)
438 {
439 fprintf(out,"%2.3f %6d %6d %9ld %9ld\n", (float)i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);
440 }
441 fclose(out);
442  
443 j=0;
444 m=0;
445 out=fopen("/tmp/plane_cut.txt","w"); // writes echo_map - e.g. density map to file
446 for (i=0;i < map_size; i++)
447 {
448 fprintf(out,"% 2.5f %2.5f %8.2f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);
449 j+=3;
450 //m++;
451 //if (m > 1){ fprintf(out,"\n"); m=0;} //make isoline for gnuplot.
452 }
453  
454 /* for (i=0; i < period_size; i++)
455 {
456 a=k*i;
457 for(j=0; j < period_size; j++)
458 {
459 b=k*j;
460 if( ((b+a) >= (Xr-Xl)) && (b <= ((Xr-Xl)+a)) && (a <= ((Xr-Xl)+b)) ) // kontrola trojuhelnikove nerovnosti
461 {
462 fprintf(out,"% 4.3f %4.3f %8.2f\n",(a*((a-b)*b + Xl*Xl)-b*Xr*Xr)/(2*a*Xl-2*b*Xr),sqrt( ((-b*b+Xl*Xl)*(a-Xr)*(a-b+Xl-Xr)*(a+Xr)*(a-b-Xl+Xr))/(4*(a*Xl-b*Xr)*(a*Xl-b*Xr)) ),(correlationl[i]+correlationr[j])/2);
463 }
464 }
465 fprintf(out, "\n");
466 }*/
467 fclose(out);
468  
469 out=fopen("/tmp/chirp.txt","w");
470 for (i=0; i <= (chirp_size - 1); i++)
471 {
472 fprintf(out,"%6d %6d\n", i, chirp[i]);
473 }
474 fclose(out);
475  
476 out=fopen("/tmp/echo.txt","w");
477 for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);
478 fclose(out);
479  
480 out=fopen("/tmp/spektra.txt","w");
481 for (i=0; i < frequency_bins; i++)
482 {
483 fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);
484 }
485 fclose(out);
486  
487 printf("Job done.\n");
488  
489 free(correlationl);
490 free(correlationr);
491 free(L_signal);
492 free(R_signal);
493 free(chirp);
494 free(signal);
495 free(echo_map);
496  
497 snd_pcm_close(playback_handle);
498 snd_pcm_close(capture_handle);
499 return 0;
500 }
501