///////////////////////////////////////////////////////////////////////////////////// A small demo of sonar.// Program allow distance measuring.// Uses cross-correlation algorithm to find echos//// Author: kaklik (kaklik@mlab.cz)//$Id:$///////////////////////////////////////////////////////////////////////////////////#include <stdio.h>#include <stdlib.h>#include <string.h>#include <sched.h>#include <errno.h>#include <getopt.h>#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 5.0 // maximal working radius in meters#define Xl -0.1 // microphones position#define Xr 0.1static 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 */static unsigned int buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */static int resample = 1; /* enable alsa-lib resampling */unsigned int chirp_size;static snd_pcm_sframes_t buffer_size; // size of buffer at sound cardstatic snd_pcm_sframes_t period_size; //samples per framestatic snd_output_t *output = NULL;static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels){unsigned int rrate;snd_pcm_uframes_t size;int err, dir;/* choose all parameters */err = snd_pcm_hw_params_any(handle, params);if (err < 0){printf("Broken configuration for playback: no configurations available: %s\n", snd_strerror(err));return err;}/* set hardware resampling */err = snd_pcm_hw_params_set_rate_resample(handle, params, resample);if (err < 0){printf("Resampling setup failed for playback: %s\n", snd_strerror(err));return err;}/* set the interleaved read/write format */err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);if (err < 0){printf("Access type not available for playback: %s\n", snd_strerror(err));return err;}/* set the sample format */err = snd_pcm_hw_params_set_format(handle, params, format);if (err < 0){printf("Sample format not available for playback: %s\n", snd_strerror(err));return err;}/* set the count of channels */err = snd_pcm_hw_params_set_channels(handle, params, channels);if (err < 0){printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err));return err;}/* set the stream rate */rrate = rate;err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);if (err < 0){printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err));return err;}if (rrate != rate){printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err);return -EINVAL;}else printf("Rate set to %i Hz\n", rate, err);/* set the buffer time */err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir);if (err < 0){printf("Unable to set buffer time %i for playback: %s\n", buffer_time, snd_strerror(err));return err;}err = snd_pcm_hw_params_get_buffer_size(params, &size);if (err < 0){printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));return err;}buffer_size = size;printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time);// set the period timeerr = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir);if (err < 0){printf("Unable to set period time %i for playback: %s\n", period_time, snd_strerror(err));return err;}err = snd_pcm_hw_params_get_period_size(params, &size, &dir);if (err < 0){printf("Unable to get period size for playback: %s\n", snd_strerror(err));return err;}period_size = size;printf("Period size set to: %d Requested period time: %ld \n", (int) period_size, (long) period_time);/* write the parameters to device */err = snd_pcm_hw_params(handle, params);if (err < 0){printf("Unable to set hw params for playback: %s\n", snd_strerror(err));return err;}return 0;}static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams){int err;/* get the current swparams */err = snd_pcm_sw_params_current(handle, swparams);if (err < 0){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 caseerr = snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size);if (err < 0){printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err));return err;}err = snd_pcm_sw_params_set_period_event(handle, swparams, 1);if (err < 0){printf("Unable to set period event: %s\n", snd_strerror(err));return err;}/* write the parameters to the playback device */err = snd_pcm_sw_params(handle, swparams);if (err < 0){printf("Unable to set sw params for playback: %s\n", snd_strerror(err));return err;}return 0;}////// SIGNAL GENERATION STUFFunsigned 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 frequencystatic const float fmax = 10000; //ending frequencystatic const float Tw = 0.0015; // time width of ping in secondsstatic float k;unsigned int n=0;double t;unsigned int chirp_samples; // number of samples per periodk=2*(fmax-f0)/Tw;chirp_samples = ceil(rate*Tw); // compute size of ping sinal in samplesfor (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))) ); // signal generation formula}return (chirp_samples); // return count of samples in ping}int main(int argc, char *argv[]){snd_pcm_t *playback_handle, *capture_handle;int err;snd_pcm_hw_params_t *hwparams;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;float a,b; // sides of trilateration triangle.float f,g; //measured lenght path of signalunsigned int i,j,m,n;unsigned int map_size; //number of points in echo map.unsigned int delayl[10],delayr[10]; //store delay of signifed correlationlong int l,r; // store correlation at strict timedouble df; //frequency resolutiondouble k; // sample numbers to distance normalising constantunsigned int frequency_bins; // number of output frequency binsdouble *inchirp; // Fourier transform variablesfftw_complex *outchirp;fftw_plan fft_plan_chirp;FILE *out; // dummy variable for file data outputsnd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registerssnd_pcm_sw_params_alloca(&swparams);printf("Simple PC sonar $Rev:$ starting work.. \n");//open and set playback deviceif ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0){printf("Playback open error: %s\n", snd_strerror(err));return 0;}if ((err = set_hwparams(playback_handle, hwparams, 1)) < 0){printf("Setting of hwparams failed: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}if ((err = set_swparams(playback_handle, swparams)) < 0){printf("Setting of swparams failed: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}//open and set capture deviceif ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0){printf("Playback open error: %s\n", snd_strerror(err));return 0;}if ((err = set_hwparams(capture_handle, hwparams, 2)) < 0){printf("Setting of hwparams failed: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}if ((err = set_swparams(capture_handle, swparams)) < 0){printf("Setting of swparams failed: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}/* err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback togetherif (err < 0){printf("Device linking error: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}*/k = SOUND_SPEED/rate; // normalising constant - normalise sample number to distancecorrelationl = malloc(period_size * sizeof(long int)); //array to store correlation curvecorrelationr = malloc(period_size * sizeof(long int)); //array to store correlation curveL_signal = malloc(period_size * sizeof(int));R_signal = malloc(period_size * sizeof(int));chirp = calloc(2*period_size, sizeof(short));signal = malloc(2*period_size * sizeof(short));map_size=0;for (i=0;i < period_size; i++) // brute force function for compute number of points in echo map.{a=k*i;for(j=0;j < period_size; j++){b=k*j;if( (Xl <= a) && (Xr <= b) ) map_size++;}}echo_map = malloc((3*map_size) * sizeof(float)); // Array to store 2D image of echosif (echo_map == NULL) printf("Can't allocate enought memory");// generate ping patternchirp_size = linear_windowed_chirp(chirp);frequency_bins = chirp_size / 2 + 1;df = (double) rate / (double) chirp_size;chirp_spect = malloc(frequency_bins * sizeof(float));lecho_spect = malloc(frequency_bins * sizeof(float));recho_spect = malloc(frequency_bins * sizeof(float));inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFToutchirp = 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 buffererr = snd_pcm_writei(playback_handle, chirp, period_size);if (err < 0){printf("Initial write error: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}//start sreamerr = snd_pcm_start(playback_handle);if (err < 0){printf("Start error: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}err = snd_pcm_start(capture_handle);if (err < 0){printf("Start error: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}else printf("Transmitting all samples of chirp\n");//--------------while ( snd_pcm_avail_update(capture_handle) < period_size) // wait for one period of data{usleep(1000);printf(".");}err = snd_pcm_drop(playback_handle); // stop audio streamerr = snd_pcm_drain(capture_handle);if (err < 0){printf("Stop error: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}err = snd_pcm_readi(capture_handle, signal, period_size); //read period from audio bufferif (err < 0){printf("Read error: %s\n", snd_strerror(err));exit(EXIT_FAILURE);}j=0;for (i=0;i < period_size;i++) // separe inretleaved samples to two arrays{L_signal[i]=signal[j];R_signal[i]=signal[j+1];j+=2;}printf("\nChirp transmitted \ncorrelating\n");for (n=0; n < (period_size - chirp_size - 1); n++){l=0;r=0;for ( m = 0; m < chirp_size;m++){l += chirp[m]*L_signal[m+n]; // correlate with left channelr += chirp[m]*R_signal[m+n]; // correlate with right channel}correlationl[n]=abs(l);correlationr[n]=abs(r);}m=0;printf("Building echo map\n"); // compute map from left and right correlation datafor (i=0; i < period_size; i++){f=k*i; // transform number of sample to distance (divide by 2 becouse path of signal is aproximmately 2times longer than distance)for(j=0; j < period_size; j++){g=k*j;a=(2*f*g*Xl+f*f*Xr+Xl*(g*g+(Xl-Xr)*Xr))/(2*g*Xl+2*f*Xr);b=(g*g*Xl-2*f*g*Xr+Xr*(f*f+Xl*(-Xl+Xr)))/(2*g*Xl-2*f*Xr);if( ((Xr-Xl) <= a+b) && (b <= a+(Xr-Xl)) && (a <= b+(Xr-Xl)) ) // kontrola trojuhelnikove nerovnosti{printf("%f %f\n",a,b);echo_map[m]=(f*((f-g)*g + Xl*Xl)-g*Xr*Xr)/(2*f*Xl-2*g*Xr);echo_map[m+1]=sqrt( ((-g*g+Xl*Xl)*(f-Xr)*(f-g+Xl-Xr)*(f+Xr)*(f-g-Xl+Xr))/(4*(f*Xl-g*Xr)*(f*Xl-g*Xr)) );echo_map[m+2]=(correlationl[i]+correlationr[j])/2;m+=3;}}}printf("Searching echos\n");r=0;l=0;for (n=0; n < period_size;n++) //najde nejvetsi korelace{if (l < correlationl[n]){delayl[1] = n;l = correlationl[n];}if (r < correlationr[n]){delayr[1] = n;r = correlationr[n];}}//spocitej frekvencni spektrum pro levy kanalfor(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]);// napln pole daty z praveho kanalu a spocitej frekvencni spektrumfor(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];fftw_execute(fft_plan_chirp);for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);printf("Writing output files\n");out=fopen("/tmp/sonar.txt","w");for (i=0; i <= (period_size - 1); i++){fprintf(out,"%2.3f %6d %6d %9ld %9ld\n", (float)i*k, L_signal[i], R_signal[i], correlationl[i], correlationr[i]);}fclose(out);j=0;m=0;out=fopen("/tmp/plane_cut.txt","w"); // writes echo_map - e.g. density map to filefor (i=0;i < map_size; i++){fprintf(out,"% 2.5f %2.5f %8.2f\n", echo_map[j], echo_map[j+1], echo_map[j+2]);j+=3;//m++;//if (m > 1){ fprintf(out,"\n"); m=0;} //make isoline for gnuplot.}/* for (i=0; i < period_size; i++){a=k*i;for(j=0; j < period_size; j++){b=k*j;if( ((b+a) >= (Xr-Xl)) && (b <= ((Xr-Xl)+a)) && (a <= ((Xr-Xl)+b)) ) // kontrola trojuhelnikove nerovnosti{fprintf(out,"% 4.3f %4.3f %8.2f\n",(a*((a-b)*b + Xl*Xl)-b*Xr*Xr)/(2*a*Xl-2*b*Xr),sqrt( ((-b*b+Xl*Xl)*(a-Xr)*(a-b+Xl-Xr)*(a+Xr)*(a-b-Xl+Xr))/(4*(a*Xl-b*Xr)*(a*Xl-b*Xr)) ),(correlationl[i]+correlationr[j])/2);}}fprintf(out, "\n");}*/fclose(out);out=fopen("/tmp/chirp.txt","w");for (i=0; i <= (chirp_size - 1); i++){fprintf(out,"%6d %6d\n", i, chirp[i]);}fclose(out);out=fopen("/tmp/echo.txt","w");for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);fclose(out);out=fopen("/tmp/spektra.txt","w");for (i=0; i < frequency_bins; i++){fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);}fclose(out);printf("Job done.\n");free(correlationl);free(correlationr);free(L_signal);free(R_signal);free(chirp);free(signal);free(echo_map);snd_pcm_close(playback_handle);snd_pcm_close(capture_handle);return 0;}