///////////////////////////////////////////////////////////////////////////////////// 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 <math.h>#include <SDL.h>#include <alsa/asoundlib.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.1// Alsa settingsstatic char *alsa_device = "plughw:0,0";static snd_pcm_format_t alsa_format = SND_PCM_FORMAT_S16; /* sample format */static unsigned int alsa_rate = 96000; /* stream rate */static unsigned int alsa_buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */static unsigned int alsa_period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */static int alsa_resample = 1;#define V2D SOUND_SPEED/alsa_rate // count of values to distance// Alsa driver// Hasn't so pretty error messages, but can really cut off count of the lines#define ALSA_ERR(a) { \int err; \err = a; \if (err < 0) { \printf("alsa: " #a /* stringify a */ ": %s\n", snd_strerror(err)); \return err; \} \}static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels, unsigned int rate,int resample, unsigned int buffer_time, snd_pcm_sframes_t *buffer_size,unsigned int period_time, snd_pcm_sframes_t *period_size, snd_pcm_format_t format){unsigned int rrate;snd_pcm_uframes_t size;int dir;/* choose all parameters */ALSA_ERR(snd_pcm_hw_params_any(handle, params));/* set hardware resampling */ALSA_ERR(snd_pcm_hw_params_set_rate_resample(handle, params, resample));/* set the interleaved read/write format */ALSA_ERR(snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED));/* set the sample format */ALSA_ERR(snd_pcm_hw_params_set_format(handle, params, format));/* set the count of channels */ALSA_ERR(snd_pcm_hw_params_set_channels(handle, params, channels));/* set the stream rate */rrate = rate;ALSA_ERR(snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0));if (rrate != rate) {printf("alsa: rate doesn't match (requested %iHz, get %iHz)\n", rate, rrate);return -EINVAL;} else {printf("alsa: rate set to %i Hz\n", rate);}/* set the buffer time */ALSA_ERR(snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir));ALSA_ERR(snd_pcm_hw_params_get_buffer_size(params, &size));*buffer_size = size;printf("alsa: bufffer size set to: %d requested buffer time: %ld \n", (int) *buffer_size,(long) buffer_time);// set the period timeALSA_ERR(snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir));ALSA_ERR(snd_pcm_hw_params_get_period_size(params, &size, &dir));*period_size = size;printf("alsa: period size set to: %d requested period time: %ld \n", (int) *period_size,(long) period_time);/* write the parameters to device */ALSA_ERR(snd_pcm_hw_params(handle, params));return 0;}static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams, unsigned int buffer_size){/* get the current swparams */ALSA_ERR(snd_pcm_sw_params_current(handle, swparams));// start the transfer when the buffer is almost full: never for our caseALSA_ERR(snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size));ALSA_ERR(snd_pcm_sw_params_set_period_event(handle, swparams, 1));/* write the parameters to the playback device */ALSA_ERR(snd_pcm_sw_params(handle, swparams));return 0;}unsigned int linear_windowed_chirp(short *pole) // generate the ping signal{unsigned int maxval = (1 << (snd_pcm_format_width(alsa_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(alsa_rate*Tw); // compute size of ping sinal in samplesfor (n=0;n<=chirp_samples;n++){t = (double) n / (double)alsa_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;snd_pcm_hw_params_t *hwparams;snd_pcm_sw_params_t *swparams;snd_pcm_sframes_t buffer_size; // size of buffer at sound cardsnd_pcm_sframes_t period_size; // samples per frameint err, j, l, i, r;unsigned int n, m;printf("Simple PC sonar $Rev:$ starting work.. \n");printf("Initializing ALSA.. \r");fflush(stdout);snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registerssnd_pcm_sw_params_alloca(&swparams);ALSA_ERR(snd_pcm_open(&playback_handle, alsa_device, SND_PCM_STREAM_PLAYBACK, 0));ALSA_ERR(set_hwparams(playback_handle, hwparams, 1, alsa_rate, alsa_resample,alsa_buffer_time, &buffer_size, alsa_period_time, &period_size, alsa_format));ALSA_ERR(set_swparams(playback_handle, swparams, buffer_size));ALSA_ERR(snd_pcm_open(&capture_handle, alsa_device, SND_PCM_STREAM_CAPTURE, 0));ALSA_ERR(set_hwparams(capture_handle, hwparams, 2, alsa_rate, alsa_resample,alsa_buffer_time, &buffer_size, alsa_period_time, &period_size, alsa_format));ALSA_ERR(set_swparams(capture_handle, swparams, buffer_size));printf("Initializing ALSA.. done!\n");short *chirp = calloc(2*period_size, sizeof(short));short *signal = malloc(2*period_size * sizeof(short));short *signal_l = malloc(period_size * sizeof(short));short *signal_r = malloc(period_size * sizeof(short));long int *correlation_l = malloc(period_size * sizeof(long int)); // arrays to store correlation curvelong int *correlation_r = malloc(period_size * sizeof(long int));float *echo_map = malloc(period_size * period_size * sizeof(float));unsigned char *echo_map_ = malloc(400 * 400 * sizeof(unsigned char));if (!chirp || !signal) {printf("Can't allocate buffers!\n");}printf("Generating ping signal.. \r");fflush(stdout);unsigned int chirp_size = linear_windowed_chirp(chirp);printf("Generating ping signal.. done!\n");SDL_Surface *sdl_screen;{SDL_Init(SDL_INIT_VIDEO);sdl_screen = SDL_SetVideoMode(400, 400, 32, SDL_SWSURFACE);}while (1) {ALSA_ERR(snd_pcm_writei(playback_handle, chirp, period_size));ALSA_ERR(snd_pcm_start(playback_handle));ALSA_ERR(snd_pcm_start(capture_handle));while (snd_pcm_avail_update(capture_handle) < period_size) {usleep(1000);}ALSA_ERR(snd_pcm_drop(playback_handle));ALSA_ERR(snd_pcm_drain(capture_handle));ALSA_ERR(snd_pcm_readi(capture_handle, signal, period_size));for (i = 0; i < period_size * 2; i += 2) {signal_l[i >> 1] = signal[i];signal_r[i >> 1] = signal[i + 1];}{printf("Chirp 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] * signal_l[m + n];// correlate with left channelr += chirp[m] * signal_r[m + n];// correlate with right channel}correlation_l[n] = abs(l);correlation_r[n] = abs(r);}for (i = 0; i < period_size * period_size; i += 1)echo_map[i] = 0.0f;for (i = 0; i < 400 * 400; i += 1)echo_map_[i] = 0;for (i = 0; i < period_size; i += 1) {double f = V2D * i;for (j = 0; j < period_size; j += 1) {double g = V2D * j;double a, b;/*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);*/a = f - sqrt(pow(g * g * Xl - Xr * (f * f + Xl *(-Xl + Xr)), 2) / pow(g * Xl - f * Xr, 2)) / 2.0;b = g - sqrt(pow(g * g * Xl - Xr * (f * f + Xl *(-Xl + Xr)), 2) / pow(g * Xl - f * Xr, 2)) / 2.0;if (((Xr-Xl)<=a+b) && (b<=a+(Xr-Xl)) && (a<=b+(Xr-Xl))) {/*float x = (f*((f-g)*g + Xl*Xl)-b*Xr*Xr)/(2*f*Xl-2*g*Xr);float y = 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)));*/errno = 0;float x = (f * f * g - g * Xl * Xl + f* (- g * g + Xr * Xr))/ (-2 * g * Xl + 2 * f * Xr);float y = sqrt(-((g * g * (f * f - Xl * Xl)* (f * f - 2 * f * g + g * g- (Xl - Xr) * (Xl - Xr)) * (g * g - Xr * Xr))/ pow(g * Xl - f * Xr, 2)))/(2.0 * g);/*if (x < 0)printf("%f\n", x);*/if ((x == x) && (y == y) && (errno != EDOM)) // check for NaN (NaN == NaN is false)echo_map[period_size * ((int) (y / ((float) V2D)))+ ((int) (x / ((float) V2D))+ (period_size/2))] =sqrt(correlation_l[i]/2)*sqrt(correlation_r[j]/2);}}}for (i = 0; i < period_size; i += 1) {for (j = 0; j < period_size; j += 1) {int y = i * 400 / period_size;int x = j * 400 / period_size;Uint8 green = echo_map[i*period_size+j]/(2000000000/255);Uint8 *pointer = &echo_map_[y*400+x];if (green > *pointer)*pointer = green;}}for (i = 0; i < 400; i += 1) {for (j = 0; j < 400; j += 1) {Uint32* bufp = (Uint32 *)sdl_screen->pixels + i*sdl_screen->pitch/4 + j;*bufp = SDL_MapRGB(sdl_screen->format, 0, echo_map_[i*400+j], 0);}}SDL_UpdateRect(sdl_screen, 0, 0, 400, 400);SDL_Event sdl_event;while (SDL_PollEvent(&sdl_event)) {if (sdl_event.type == SDL_QUIT)return 0;}/*FILE *out = fopen("output", "w");for (i = 0; i <= (period_size - 1); i++) {fprintf(out, "%2.3f %6d %6d %9ld %9ld\n", (float)i*V2D, signal_l[i], signal_r[i], correlation_l[i], correlation_r[i]);}fclose(out);*/}ALSA_ERR(snd_pcm_prepare(playback_handle));ALSA_ERR(snd_pcm_prepare(capture_handle));}return 0;}