| 20,8 → 20,8 |
|---|
| |
| #define SOUND_SPEED 340.0 // sound speed in air in metrs per second |
| #define MAX_RANGE 10.0 // maximal working radius in meters |
| #define Xl -0.1 // microphones position |
| #define Xr 0.1 |
| //#define Xl -0.1 // microphones position |
| //#define Xr 0.1 |
| |
| static char *device = "plughw:0,0"; /* playback device */ |
| static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */ |
| 108,7 → 108,7 |
|---|
| printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time); |
| |
| |
| /// set the period time |
| // set the period time |
| err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir); |
| if (err < 0) |
| { |
| 172,13 → 172,13 |
|---|
| } |
| |
| ////// SIGNAL GENERATION STUFF |
| unsigned int linear_windowed_chirp(short *pole) |
| unsigned int linear_windowed_chirp(short *pole) // generate the ping signal |
| { |
| unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1; |
| |
| static const float f0 = 5000; //starting frequency |
| static const float fmax = 10000; //ending frequency |
| static const float Tw = 0.0015; |
| static const float Tw = 0.0015; // time width of ping in seconds |
| static float k; |
| |
| unsigned int n=0; |
| 186,14 → 186,14 |
|---|
| unsigned int chirp_samples; // number of samples per period |
| |
| k=2*(fmax-f0)/Tw; |
| chirp_samples = ceil(rate*Tw); |
| chirp_samples = ceil(rate*Tw); // compute size of ping sinal in samples |
| |
| for (n=0;n<=chirp_samples;n++) |
| { |
| t = (double) n / (double)rate; |
| 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))) ); |
| 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 |
| } |
| return (chirp_samples); |
| return (chirp_samples); // return count of samples in ping |
| } |
| |
| int main(int argc, char *argv[]) |
| 208,7 → 208,7 |
|---|
| int *L_signal, *R_signal; |
| short *chirp, *signal; |
| float *chirp_spect, *lecho_spect, *recho_spect; |
| float a,b; |
| float a,b, Xl, Xr; |
| unsigned int i,j,m,n; |
| unsigned int delayl[10],delayr[10]; //store delay of signifed correlation |
| long int l,r; // store correlation at strict time |
| 216,13 → 216,13 |
|---|
| double k; // sample numbers to distance normalising constant |
| unsigned int frequency_bins; // number of output frequency bins |
| |
| double *inchirp; |
| double *inchirp; // Fourier transform variables |
| fftw_complex *outchirp; |
| fftw_plan fft_plan_chirp; |
| |
| FILE *out; |
| FILE *out; // dummy variable for file data output |
| |
| snd_pcm_hw_params_alloca(&hwparams); |
| snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registers |
| snd_pcm_sw_params_alloca(&swparams); |
| |
| printf("Simple PC sonar $Rev:$ starting work.. \n"); |
| 279,7 → 279,7 |
|---|
| echo_map = malloc(3*period_size*period_size * sizeof(float)); // Array to store two dimensional image of echos |
| if (echo_map == NULL) printf("Can't allocate enought memory"); |
| |
| k = SOUND_SPEED/rate; // normalising constant |
| k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distance |
| |
| // generate ping pattern |
| chirp_size = linear_windowed_chirp(chirp); |
| 367,24 → 367,34 |
|---|
| correlationl[n]=abs(l); |
| correlationr[n]=abs(r); |
| } |
| |
| Xl=-0.1; |
| Xr=0.1; |
| m=0; |
| printf("Building echo map\n"); // compute map from left and right correlation data |
| for (i=0;i < period_size; i++) |
| for (i=0;i < 200; i++) |
| { |
| a=k*i; |
| for(j=0;j < period_size; j++) |
| for(j=0;j < 200; j++) |
| { |
| b=k*j; |
| if( (b+a) >= (Xr-Xl)) |
| if( (b+a) >= (Xr-Xl) ) |
| { |
| echo_map[m]=a; ///(-a*a+b*b+Xl*Xl+Xr*Xr)/(2*Xl-2*Xr); |
| echo_map[m+1]=sqrt((a-b-Xl-Xr)*(a+b+Xl+Xr)*(a-b-Xl+Xr)*(a-b-Xl+Xr)*(a+b-Xl+Xr))/(-2*(Xl-Xr)*(Xl-Xr)); |
| //a=10; |
| //b=0; |
| echo_map[m]=-((a*a)-(b*b)+(Xl-Xr)*(Xl-Xr))/(2*(Xl-Xr)); |
| echo_map[m+1]=sqrt((a-b-Xl-Xr)*(a+b+Xl+Xr)*(a-b-Xl+Xr)*(a-b-Xl+Xr)*(a+b-Xl+Xr)/(-2*(Xl-Xr)*(Xl-Xr))); |
| echo_map[m+2]=correlationl[i]+correlationr[j]; |
| m+=3; |
| } |
| } |
| } |
| j=0; |
| for (i=300;i < 700; i++) // print some debugg data from echo map |
| { |
| printf("% 4.3f %4.3f %6.3f\n", echo_map[j], echo_map[j+1], echo_map[j+2]); |
| j+=3; |
| } |
| |
| printf("Searching echos\n"); |
| r=0; |
| l=0; |