4,7 → 4,7 |
// Uses cross-correlation algorithm to find echos |
// |
// Author: kaklik (kaklik@mlab.cz) |
// |
//$Id:$ |
/////////////////////////////////////////////////////////////////////////////////// |
|
#include <stdio.h> |
21,10 → 21,7 |
#define SOUND_SPEED 340.0 // sound speed in air in metrs per second |
#define MAX_RANGE 10.0 // maximal working radius in meters |
#define APERTURE 0.2 // distance between microphones |
#define MAP_SIZE 100 |
|
#define RESOLUTION 100 // resolution per map pixel |
|
static char *device = "plughw:0,0"; /* playback device */ |
static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */ |
static unsigned int rate = 96000; /* stream rate */ |
206,6 → 203,7 |
snd_pcm_sw_params_t *swparams; |
|
long int *correlationl, *correlationr; |
float *echo_map; |
int *L_signal, *R_signal; |
short *chirp, *signal; |
float *chirp_spect, *lecho_spect, *recho_spect; |
216,8 → 214,6 |
double df; //frequency resolution |
unsigned int frequency_bins; // number of output frequency bins |
|
float density_map[MAP_SIZE][MAP_SIZE]; // Array to store two dimensional image of echos |
|
double *inchirp; |
fftw_complex *outchirp; |
fftw_plan fft_plan_chirp; |
227,7 → 223,7 |
snd_pcm_hw_params_alloca(&hwparams); |
snd_pcm_sw_params_alloca(&swparams); |
|
printf("Simple PC sonar ver. 000000001 starting work.. \n"); |
printf("Simple PC sonar $Rev:$ starting work.. \n"); |
|
//open and set playback device |
if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) |
278,6 → 274,7 |
R_signal = malloc(period_size * sizeof(int)); |
chirp = calloc(2*period_size, sizeof(short)); |
signal = malloc(2*period_size * sizeof(short)); |
echo_map = malloc(3*period_size * sizeof(float)); // Array to store two dimensional image of echos |
|
// generate ping pattern |
chirp_size = linear_windowed_chirp(chirp); |
366,17 → 363,18 |
correlationr[n]=abs(r); |
} |
|
m=0; |
printf("Building echo map\n"); // compute map from left and right correlation data |
for (i=0;i < MAP_SIZE; i++) |
{ |
|
for (j=0;j < MAP_SIZE; j++) |
for (i=0;i < period_size; i++) |
{ |
x=(float)i*RESOLUTION; y=(float)j*RESOLUTION; //transofm integger index of array to float with appproopirate resolution |
|
density_map[i][j]=(float)correlationl[(int)sqrt(x*x + y*y)]*correlationr[(int)sqrt(APERTURE*APERTURE - 2*APERTURE*x + x*x + y*y)]; |
for(j=0;j < period_size; j++) |
{ |
echo_map[m]=(i*i-j*j+APERTURE*APERTURE)/(2*APERTURE); |
echo_map[m+1]=sqrt(-(i-j-APERTURE)*(i+j-APERTURE)*(i-j+APERTURE)*(i+j+APERTURE))/(2*r); |
echo_map[m+2]=correlationl[i]*correlationr[j]; |
m+=3; |
} |
} |
} |
|
|
printf("Searching echos\n"); |
396,7 → 394,7 |
} |
} |
|
//spocitejj frekvencni spektrum pro levy kanal |
//spocitej frekvencni spektrum pro levy kanal |
for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i]; |
fftw_execute(fft_plan_chirp); |
for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]); |
415,11 → 413,12 |
} |
fclose(out); |
|
j=0; |
out=fopen("/tmp/plane_cut.txt","w"); // writes plane cut - e.g. density map to file |
for (i=0;i < MAP_SIZE; i++) |
for (i=0;i < period_size; i++) |
{ |
for (j=0;j < MAP_SIZE; j++) fprintf(out,"%3.2f ", density_map); |
fprintf(out,"\n"); |
fprintf(out,"%3.2f %3.2f %3.2f\n", echo_map[j], echo_map[j+1], echo_map[j+2]); |
j+=3; |
} |
|
out=fopen("/tmp/chirp.txt","w"); |
446,6 → 445,7 |
free(R_signal); |
free(chirp); |
free(signal); |
free(echo_map); |
|
snd_pcm_close(playback_handle); |
snd_pcm_close(capture_handle); |