| 1,6 → 1,9 |
|---|
| /* |
| * This small demo sends a simple sinusoidal wave to your speakers. |
| */ |
| /////////////////////////////////////////////////////////////////////////////////// |
| // This small demo of sonar. |
| // Program allow distance measuring. |
| // |
| // |
| /////////////////////////////////////////////////////////////////////////////////// |
| |
| #include <stdio.h> |
| #include <stdlib.h> |
| 27,62 → 30,13 |
|---|
| |
| int period=0; |
| int cperiod=0; |
| int chirp[1000000]; |
| short signal[44100*6]; // record 6s of input samples |
| int chirp[100000]; |
| short signal[1000000]; // record 6s of input samples |
| |
| static snd_pcm_sframes_t buffer_size; // size of buffer at sound card |
| static snd_pcm_sframes_t period_size; //samples per frame |
| static snd_output_t *output = NULL; |
| |
| /*static void generate_sine(const snd_pcm_channel_area_t *areas, |
| snd_pcm_uframes_t offset, |
| int count, double *_phase) |
| { |
| static double max_phase = 2. * M_PI; |
| double phase = *_phase; |
| double step = max_phase*freq/(double)rate; |
| double res; |
| unsigned char *samples[channels], *tmp; |
| int steps[channels]; |
| unsigned int chn, byte; |
| union { |
| int i; |
| unsigned char c[4]; |
| } ires; |
| unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1; |
| int bps = snd_pcm_format_width(format) / 8; /* bytes per sample */ |
| |
| /* verify and prepare the contents of areas |
| for (chn = 0; chn < channels; chn++) { |
| if ((areas[chn].first % 8) != 0) { |
| printf("areas[%i].first == %i, aborting...\n", chn, areas[chn].first); |
| exit(EXIT_FAILURE); |
| } |
| samples[chn] = /*(signed short *)(((unsigned char *)areas[chn].addr) + (areas[chn].first / 8)); |
| if ((areas[chn].step % 16) != 0) { |
| printf("areas[%i].step == %i, aborting...\n", chn, areas[chn].step); |
| exit(EXIT_FAILURE); |
| } |
| steps[chn] = areas[chn].step / 8; |
| samples[chn] += offset * steps[chn]; |
| } |
| /* fill the channel areas |
| while (count-- > 0) { |
| res = sin(phase) * maxval; |
| ires.i = res; |
| tmp = ires.c; |
| for (chn = 0; chn < channels; chn++) { |
| for (byte = 0; byte < (unsigned int)bps; byte++) |
| *(samples[chn] + byte) = tmp[byte]; |
| samples[chn] += steps[chn]; |
| } |
| phase += step; |
| if (phase >= max_phase) |
| phase -= max_phase; |
| } |
| *_phase = phase; |
| }*/ |
| |
| static int set_hwparams(snd_pcm_t *handle, |
| snd_pcm_hw_params_t *params, |
| snd_pcm_access_t access) |
| 226,10 → 180,9 |
|---|
| |
| }*/ |
| |
| |
| |
| unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole){ // vygeneruje linearni chirp a vzorky ulozi do pole |
| |
| // vygeneruje linearni chirp a vzorky ulozi do pole |
| unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole) |
| { |
| unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1; |
| |
| static const float f0 = 1000; |
| 241,16 → 194,17 |
|---|
| double t; |
| unsigned int perioda; |
| |
| k=2*(fmax-f0)/Tw; |
| perioda = rate*Tw; |
| k=2*(fmax-f0)/Tw; |
| perioda = rate*Tw; |
| |
| for(n=0;n<=perioda;n++){ |
| t = (double) n/ (double)rate; |
| pole[n] = (short) round ( (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))) ); |
| } |
| return perioda; |
| for(n=0;n<=perioda;n++){ |
| t = (double) n/ (double)rate; |
| pole[n] = (short) round ( (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))) ); |
| } |
| return perioda; |
| } |
| |
| // generate sine samples and store |
| int sine(unsigned int *pole, unsigned int delka_pole) |
| { |
| unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1; |
| 301,7 → 255,7 |
|---|
| int err; |
| |
| avail = snd_pcm_avail_update(handle); |
| while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) { |
| while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) { // segmentation fault checking disabled |
| |
| err = snd_pcm_readi(handle, (signal+cperiod*period_size), period_size); |
| if (err < 0) { |
| 331,15 → 285,17 |
|---|
| struct async_private_data data; |
| snd_async_handler_t *chandler, *phandler; |
| int count; |
| unsigned int i,j; |
| unsigned int i,j,m,n; |
| unsigned int delay; //store delay of maximal correlation |
| double r; |
| double correlation[1000000]; //array to store correlation curve |
| |
| FILE *out; |
| |
| |
| snd_pcm_hw_params_alloca(&hwparams); |
| snd_pcm_sw_params_alloca(&swparams); |
| |
| //open adn set playback device |
| //open and set playback device |
| if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) { |
| printf("Playback open error: %s\n", snd_strerror(err)); |
| return 0; |
| 419,18 → 375,38 |
|---|
| exit(EXIT_FAILURE); |
| } |
| |
| |
| |
| /* because all other work is done in the signal handler, |
| suspend the process */ |
| //wait until all samples aren't transmitted |
| printf("Waiting for transmitt all samples\n"); |
| while(cperiod<10) { |
| sleep(1); |
| printf("."); |
| } |
| printf("\nData transmitted... \nprocessing...\n"); |
| |
| out=fopen("./output.txt","w"); |
| for(i=0;i<=100000;i++) fprintf(out,"%6d %6d \n",chirp[i],signal[i]); |
| for(i=0;i<=100000;i++) fprintf(out,"%6d %6d %6d \n",i,chirp[i],signal[i]); |
| fclose(out); |
| |
| #define SIGNAL_SAMPLES 100000 |
| #define SAMPLES 1000 |
| |
| for(n=0; n < (SIGNAL_SAMPLES - SAMPLES);n++){ //spocita korelaci pro mozna spozdeni |
| r=0; |
| for(m=0;m < SAMPLES;m++) r += chirp[m]*signal[m+n]; |
| correlation[n]=r; |
| } |
| |
| r=0; |
| for(n=0; n < (SIGNAL_SAMPLES - SAMPLES);n++){ //najde nejvetsi shodu (pro nazornost v samostatnem cyklu) |
| if (r < correlation[n]){ |
| delay = n; |
| r = correlation[n]; |
| } |
| } |
| |
| printf("\nEcho zacina na: %d vzorku.\n", delay); |
| printf("\nCasove na: %ds", delay); |
| |
| snd_pcm_close(playback_handle); |
| snd_pcm_close(capture_handle); |
| return 0; |