| 16,6 → 16,7 |
|---|
| #include <alsa/asoundlib.h> |
| #include <sys/time.h> |
| #include <math.h> |
| #include <fftw3.h> |
| |
| #define SOUND_SPEED 340.0 // sound speed in air in metrs per second |
| #define MAX_RANGE 10.0 // maximal working radius in meters |
| 143,7 → 144,7 |
|---|
| printf("Unable to determine current swparams for playback: %s\n", snd_strerror(err)); |
| return err; |
| } |
| // start the transfer when the buffer is almost full: never fou our case |
| // start the transfer when the buffer is almost full: never fou our case |
| err = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size); |
| if (err < 0) |
| { |
| 203,10 → 204,17 |
|---|
| long int *correlationl, *correlationr; |
| int *L_signal, *R_signal; |
| short *chirp, *signal; |
| float *chirp_spect, *echo_spect; |
| unsigned int i,j,m,n; |
| unsigned int delay[10]; //store delay of signifed correlation |
| unsigned int delayl[10],delayr[10]; //store delay of signifed correlation |
| long int l,r; // store correlation at strict time |
| double df; //frequency resolution |
| unsigned int frequency_bins; // number of output frequency bins |
| |
| double *inchirp; |
| fftw_complex *outchirp; |
| fftw_plan fft_plan_chirp; |
| |
| FILE *out; |
| |
| snd_pcm_hw_params_alloca(&hwparams); |
| 250,12 → 258,12 |
|---|
| exit(EXIT_FAILURE); |
| } |
| |
| /* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together |
| if (err < 0) |
| { |
| printf("Device linking error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| }*/ |
| /* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together |
| if (err < 0) |
| { |
| printf("Device linking error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| }*/ |
| |
| correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve |
| correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve |
| 267,6 → 275,22 |
|---|
| // generate ping pattern |
| chirp_size = linear_windowed_chirp(chirp); |
| |
| frequency_bins = chirp_size / 2 + 1; |
| df = (double) rate / (double) chirp_size; |
| chirp_spect = malloc(frequency_bins * sizeof(float)); |
| echo_spect = malloc(frequency_bins * sizeof(float)); |
| |
| inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT |
| outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins); |
| |
| fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE); |
| |
| printf("compute chirp spectrum\n"); |
| for(i=0; i < chirp_size; i++) inchirp[i] = chirp[i]; |
| fftw_execute(fft_plan_chirp); |
| for(i=0; i < frequency_bins; i++) chirp_spect[i] = sqrt( outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1] ); |
| |
| // write chirp data to souncard buffer |
| err = snd_pcm_writei(playback_handle, chirp, period_size); |
| if (err < 0) |
| { |
| 290,7 → 314,6 |
|---|
| } |
| else printf("Waiting for transmitt all samples\n"); |
| //-------------- |
| |
| while ( snd_pcm_avail_update(capture_handle) < period_size) |
| { |
| usleep(1000); |
| 325,13 → 348,13 |
|---|
| { |
| l=0; |
| r=0; |
| for (m=0;m < chirp_size;m++) |
| for ( m = 0; m < chirp_size;m++) |
| { |
| l += chirp[m]*L_signal[m+n]; // correlate with left channel |
| r += chirp[m]*R_signal[m+n]; // correlate with right channel |
| } |
| correlationl[n]=l; |
| correlationr[n]=r; |
| correlationl[n]=abs(l); |
| correlationr[n]=abs(r); |
| } |
| |
| printf("Searching echos\n"); |
| 341,30 → 364,48 |
|---|
| { |
| if (l < correlationl[n]) |
| { |
| delay[1] = n; |
| delayl[1] = n; |
| l = correlationl[n]; |
| } |
| if (r < correlationr[n]) |
| { |
| delay[2] = n; |
| delayr[1] = n; |
| r = correlationr[n]; |
| } |
| } |
| |
| |
| |
| printf("Writing output file\n"); |
| out=fopen("/tmp/sonar.txt","w"); |
| j=0; |
| for (i=0; i <= (period_size -1); i++) |
| for (i=0; i <= (period_size - 1); i++) |
| { |
| fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]); |
| fprintf(out,"%6d %6d %6d %9ld %9ld\n",i,L_signal[i],R_signal[i],correlationl[i], correlationr[i]); |
| j+=2; |
| } |
| fclose(out); |
| |
| printf("Echo zacina na: %d vzorku.\n", delay[1]); |
| printf("Casove na: %f s\n", ((float)delay[1]/rate)); |
| printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delay[1]/rate)); |
| out=fopen("/tmp/chirp.txt","w"); |
| j=0; |
| for (i=0; i <= (chirp_size - 1); i++) |
| { |
| fprintf(out,"%6d %6d %f\n", i, chirp[i], chirp_spect[i]); |
| j+=2; |
| } |
| fclose(out); |
| |
| printf("Echo zacina na: %d vzorku.\n", delayl[1]); |
| printf("Casove na: %f s\n", ((float)delayl[1]/rate)); |
| printf("vzdalenost: %f m\n", (SOUND_SPEED*(float)delayl[1]/rate)); |
| |
| free(correlationl); |
| free(correlationr); |
| free(L_signal); |
| free(R_signal); |
| free(chirp); |
| free(signal); |
| |
| snd_pcm_close(playback_handle); |
| snd_pcm_close(capture_handle); |
| return 0; |