| 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); |