| 0,0 → 1,318 |
|---|
| /////////////////////////////////////////////////////////////////////////////////// |
| // 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 settings |
| static 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 time |
| ALSA_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 case |
| ALSA_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 frequency |
| static const float fmax = 10000; // ending frequency |
| static const float Tw = 0.0015; // time width of ping in seconds |
| static float k; |
| |
| unsigned int n=0; |
| double t; |
| unsigned int chirp_samples; //number of samples per period |
| |
| k=2*(fmax-f0)/Tw; |
| chirp_samples = ceil(alsa_rate*Tw); // compute size of ping sinal in samples |
| |
| for (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 card |
| snd_pcm_sframes_t period_size; // samples per frame |
| |
| int 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 registers |
| snd_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 curve |
| long 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 channel |
| r += 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; |
| } |