| Line 21... |
Line 21... |
| 21 |
#define SOUND_SPEED 340.0 // sound speed in air in metrs per second
|
21 |
#define SOUND_SPEED 340.0 // sound speed in air in metrs per second
|
| 22 |
#define MAX_RANGE 10.0 // maximal working radius in meters
|
22 |
#define MAX_RANGE 10.0 // maximal working radius in meters
|
| 23 |
#define APERTURE 0.2 // distance between microphones
|
23 |
#define APERTURE 0.2 // distance between microphones
|
| 24 |
#define MAP_SIZE 100
|
24 |
#define MAP_SIZE 100
|
| 25 |
|
25 |
|
| - |
|
26 |
#define RESOLUTION 1/100 // resolution in metres per map pixel
|
| - |
|
27 |
|
| 26 |
static char *device = "plughw:0,0"; /* playback device */
|
28 |
static char *device = "plughw:0,0"; /* playback device */
|
| 27 |
static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */
|
29 |
static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */
|
| 28 |
static unsigned int rate = 96000; /* stream rate */
|
30 |
static unsigned int rate = 96000; /* stream rate */
|
| 29 |
static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */
|
31 |
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 */
|
32 |
static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */
|
| Line 205... |
Line 207... |
| 205 |
|
207 |
|
| 206 |
long int *correlationl, *correlationr;
|
208 |
long int *correlationl, *correlationr;
|
| 207 |
int *L_signal, *R_signal;
|
209 |
int *L_signal, *R_signal;
|
| 208 |
short *chirp, *signal;
|
210 |
short *chirp, *signal;
|
| 209 |
float *chirp_spect, *lecho_spect, *recho_spect;
|
211 |
float *chirp_spect, *lecho_spect, *recho_spect;
|
| - |
|
212 |
float x,y;
|
| 210 |
unsigned int i,j,m,n;
|
213 |
unsigned int i,j,m,n;
|
| 211 |
unsigned int delayl[10],delayr[10]; //store delay of signifed correlation
|
214 |
unsigned int delayl[10],delayr[10]; //store delay of signifed correlation
|
| 212 |
long int l,r; // store correlation at strict time
|
215 |
long int l,r; // store correlation at strict time
|
| 213 |
double df; //frequency resolution
|
216 |
double df; //frequency resolution
|
| 214 |
unsigned int frequency_bins; // number of output frequency bins
|
217 |
unsigned int frequency_bins; // number of output frequency bins
|
| Line 364... |
Line 367... |
| 364 |
}
|
367 |
}
|
| 365 |
|
368 |
|
| 366 |
printf("Building echo map\n"); // compute map from left and right correlation data
|
369 |
printf("Building echo map\n"); // compute map from left and right correlation data
|
| 367 |
for (i=0;i < MAP_SIZE; i++)
|
370 |
for (i=0;i < MAP_SIZE; i++)
|
| 368 |
{
|
371 |
{
|
| - |
|
372 |
|
| - |
|
373 |
for (j=0;j < MAP_SIZE; j++)
|
| - |
|
374 |
{
|
| - |
|
375 |
x=(float)i*RESOLUTION; y=(float)j*RESOLUTION; //transofm integger index of array to float with appproopirate resolution
|
| - |
|
376 |
|
| 369 |
for (j=0;j < MAP_SIZE; j++) density_map[i][j]=correlationl[(int)sqrt(i*i + j*j)]*correlationr[(int)sqrt(APERTURE*APERTURE - 2*APERTURE*i + i*i + j*j)];
|
377 |
density_map[i][j]=(float)correlationl[(int)sqrt(x*x + y*y)]*correlationr[(int)sqrt(APERTURE*APERTURE - 2*APERTURE*x + x*x + y*y)];
|
| - |
|
378 |
}
|
| 370 |
}
|
379 |
}
|
| 371 |
|
380 |
|
| 372 |
|
381 |
|
| 373 |
printf("Searching echos\n");
|
382 |
printf("Searching echos\n");
|
| 374 |
r=0;
|
383 |
r=0;
|
| Line 404... |
Line 413... |
| 404 |
{
|
413 |
{
|
| 405 |
fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",SOUND_SPEED * (float) i / rate,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
|
414 |
fprintf(out,"%2.3f %6d %6d %9ld %9ld\n",SOUND_SPEED * (float) i / rate,L_signal[i],R_signal[i],correlationl[i], correlationr[i]);
|
| 406 |
}
|
415 |
}
|
| 407 |
fclose(out);
|
416 |
fclose(out);
|
| 408 |
|
417 |
|
| 409 |
out=fopen("/tmp/plane_cut.txt","w");
|
418 |
out=fopen("/tmp/plane_cut.txt","w"); // writes plane cut - e.g. density map to file
|
| 410 |
for (i=0;i < MAP_SIZE; i++)
|
419 |
for (i=0;i < MAP_SIZE; i++)
|
| 411 |
{
|
420 |
{
|
| 412 |
for (j=0;j < MAP_SIZE; j++) fprintf(out,"%6f", density_map);
|
421 |
for (j=0;j < MAP_SIZE; j++) fprintf(out,"%3.2f ", density_map);
|
| - |
|
422 |
fprintf(out,"\n");
|
| 413 |
}
|
423 |
}
|
| 414 |
|
424 |
|
| 415 |
out=fopen("/tmp/chirp.txt","w");
|
425 |
out=fopen("/tmp/chirp.txt","w");
|
| 416 |
for (i=0; i <= (chirp_size - 1); i++)
|
426 |
for (i=0; i <= (chirp_size - 1); i++)
|
| 417 |
{
|
427 |
{
|