| 17,23 → 17,23 |
|---|
| #include <sys/time.h> |
| #include <math.h> |
| |
| #define SOUND_SPEED 340.0 // sound speed in air in metrs per second |
| #define MAX_RANGE 10.0 // maximal working radius in meters |
| |
| static char *device = "plughw:0,0"; /* playback device */ |
| static snd_pcm_format_t format = SND_PCM_FORMAT_S16; /* sample format */ |
| static unsigned int rate = 98000; /* stream rate */ |
| static unsigned int buffer_time = 500000; /* ring buffer length in us */ |
| static unsigned int period_time = 100000; /* period time in us */ |
| static unsigned int rate = 96000; /* stream rate */ |
| static unsigned int buffer_time = MAX_RANGE / SOUND_SPEED * 1e6; /* ring buffer length in us */ |
| static unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e5; /* period time in us */ |
| static int resample = 1; /* enable alsa-lib resampling */ |
| static int period_event = 0; /* produce poll event after each period */ |
| |
| #define SOUND_SPEED 340 |
| #define SIGNAL_SAMPLES 100000 |
| #define CHIRP_OFFSET 0 |
| |
| unsigned int chirp_size; |
| |
| int period=0; |
| int cperiod=0; |
| int chirp[100000]; |
| short *chirp; |
| short signal[1000000]; // record 6s of input samples |
| |
| static snd_pcm_sframes_t buffer_size; // size of buffer at sound card |
| 48,31 → 48,36 |
|---|
| |
| /* choose all parameters */ |
| err = snd_pcm_hw_params_any(handle, params); |
| if (err < 0) { |
| 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) { |
| 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) { |
| 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) { |
| 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) { |
| if (err < 0) |
| { |
| printf("Channels count (%i) not available for playbacks: %s\n", channels, snd_strerror(err)); |
| return err; |
| } |
| 79,11 → 84,13 |
|---|
| /* set the stream rate */ |
| rrate = rate; |
| err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0); |
| if (err < 0) { |
| if (err < 0) |
| { |
| printf("Rate %iHz not available for playback: %s\n", rate, snd_strerror(err)); |
| return err; |
| } |
| if (rrate != rate) { |
| if (rrate != rate) |
| { |
| printf("Rate doesn't match (requested %iHz, get %iHz)\n", rate, err); |
| return -EINVAL; |
| } |
| 90,31 → 97,42 |
|---|
| 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) { |
| 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) { |
| if (err < 0) |
| { |
| printf("Unable to get buffer size for playback: %s\n", snd_strerror(err)); |
| return err; |
| } |
| buffer_size = size; |
| /* set the period time */ |
| printf("Bufffer size set to: %d Requested buffer time: %ld \n", (int) buffer_size, (long) buffer_time); |
| |
| |
| /// set the period time |
| err = snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir); |
| if (err < 0) { |
| 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) { |
| 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) { |
| if (err < 0) |
| { |
| printf("Unable to set hw params for playback: %s\n", snd_strerror(err)); |
| return err; |
| } |
| 127,35 → 145,30 |
|---|
| |
| /* get the current swparams */ |
| err = snd_pcm_sw_params_current(handle, swparams); |
| if (err < 0) { |
| 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: */ |
| /* (buffer_size / avail_min) * avail_min */ |
| err = snd_pcm_sw_params_set_start_threshold(handle, swparams, (buffer_size / period_size) * period_size); |
| if (err < 0) { |
| // 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) |
| { |
| printf("Unable to set start threshold mode for playback: %s\n", snd_strerror(err)); |
| return err; |
| } |
| /* allow the transfer when at least period_size samples can be processed */ |
| /* or disable this mechanism when period event is enabled (aka interrupt like style processing) */ |
| err = snd_pcm_sw_params_set_avail_min(handle, swparams, period_event ? buffer_size : period_size); |
| if (err < 0) { |
| printf("Unable to set avail min for playback: %s\n", snd_strerror(err)); |
| return err; |
| } |
| /* enable period events when requested */ |
| if (period_event) { |
| |
| err = snd_pcm_sw_params_set_period_event(handle, swparams, 1); |
| if (err < 0) { |
| 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) { |
| if (err < 0) |
| { |
| printf("Unable to set sw params for playback: %s\n", snd_strerror(err)); |
| return err; |
| } |
| 162,7 → 175,8 |
|---|
| return 0; |
| } |
| |
| struct async_private_data { |
| struct async_private_data |
| { |
| signed short *samples; |
| snd_pcm_channel_area_t *areas; |
| unsigned int period; |
| 170,22 → 184,7 |
|---|
| |
| |
| ////// SIGNAL GENERATION STUFF |
| /*int linear_chirp(int *pole, int delka_pole){ // vygeneruje linearni chirp a vzorky ulozi do pole |
| |
| static const float f0 = 0.0001; |
| static const float k = 0.00001; |
| |
| int t; |
| |
| // if((spozdeni+delka) < delka_pole) |
| for(t=0;t < delka_pole;t++) pole[t] = round ( 10000*sin(2*M_PI*(t+faze)*(f0+(k/2)*(t+faze))) ); |
| faze +=t; |
| // else return 0; |
| |
| }*/ |
| |
| // vygeneruje linearni chirp a vzorky ulozi do pole |
| unsigned int linear_windowed_chirp(unsigned int *pole, unsigned int delka_pole,unsigned int offset) |
| unsigned int linear_windowed_chirp(short *pole) |
| { |
| unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1; |
| |
| 196,43 → 195,19 |
|---|
| |
| unsigned int n=0; |
| double t; |
| unsigned int perioda; |
| unsigned int chirp_samples; // number of samples per period |
| |
| k=2*(fmax-f0)/Tw; |
| perioda = rate*Tw; |
| chirp_samples = ceil(rate*Tw); |
| |
| for(n=0;n<=perioda;n++){ |
| for (n=0;n<=chirp_samples;n++) |
| { |
| t = (double) n/ (double)rate; |
| pole[n+offset] = (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))) ); |
| 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))) ); |
| } |
| return (perioda+offset); |
| return (chirp_samples); |
| } |
| |
| // 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; |
| unsigned int n; |
| double t; |
| |
| for(n=0;n < delka_pole;n++){ |
| t = 440.0 * (double) n/ (double)rate; |
| pole[n] = (short) floor(maxval*sin(2*M_PI*t)); |
| } |
| } |
| //// generate simple sine ping |
| unsigned int sine_ping(unsigned int *pole, unsigned int delka_pole,unsigned int offset, double frequency) |
| { |
| unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1; |
| unsigned int n; |
| double t; |
| |
| for(n=0;n < delka_pole;n++){ |
| t = frequency * (double) n/ (double)rate; |
| pole[n] = (short) floor(maxval*sin(2*M_PI*t)); |
| } |
| } |
| |
| /////////// CALL BACK STUFF /////////////////// |
| static void async_playback_callback(snd_async_handler_t *ahandler) |
| { |
| 241,14 → 216,17 |
|---|
| int err; |
| |
| avail = snd_pcm_avail_update(handle); |
| while ((avail >= period_size) && ((period*period_size) < chirp_size) ) { |
| while ((avail >= period_size) && ((period*period_size) < chirp_size) ) |
| { |
| |
| err = snd_pcm_writei(handle, (chirp+period*period_size), period_size); |
| if (err < 0) { |
| if (err < 0) |
| { |
| printf("Write error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| } |
| if (err != period_size) { |
| if (err != period_size) |
| { |
| printf("Write error: written %i expected %li\n", err, period_size); |
| exit(EXIT_FAILURE); |
| } |
| 264,14 → 242,17 |
|---|
| int err; |
| |
| avail = snd_pcm_avail_update(handle); |
| while ((avail >= period_size) /*&& ((period*period_size) < (CHIRP_SIZE-100))*/ ) { // segmentation fault checking disabled |
| 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) { |
| if (err < 0) |
| { |
| printf("Read error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| } |
| if (err != period_size) { |
| if (err != period_size) |
| { |
| printf("Read error: red %i expected %li\n", err, period_size); |
| exit(EXIT_FAILURE); |
| } |
| 310,111 → 291,122 |
|---|
| printf("Simple PC sonar ver. 000000001 starting work.. \n"); |
| |
| //open and set playback device |
| if ((err = snd_pcm_open(&playback_handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) { |
| 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; |
| } |
| |
| if ((err = set_hwparams(playback_handle, hwparams, 1)) < 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) { |
| 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 device |
| if ((err = snd_pcm_open(&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0) { |
| if ((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) { |
| 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) { |
| if ((err = set_swparams(capture_handle, swparams)) < 0) |
| { |
| printf("Setting of swparams failed: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| } |
| |
| // generate ping pattern |
| chirp = malloc(2*period_size * sizeof(short)); |
| chirp_size = linear_windowed_chirp(chirp); |
| |
| chirp_size=linear_windowed_chirp(chirp,1000000, CHIRP_OFFSET); |
| |
| // register playback callback |
| err = snd_async_add_pcm_handler(&phandler, playback_handle, async_playback_callback, &data); // fill by dummy &data |
| if (err < 0) { |
| /* err = snd_async_add_pcm_handler(&phandler, playback_handle, async_playback_callback, &data); // fill by dummy &data |
| if (err < 0) |
| { |
| printf("Unable to register async handler\n"); |
| exit(EXIT_FAILURE); |
| } |
| }*/ |
| |
| if ((err = snd_pcm_prepare (playback_handle)) < 0) { |
| fprintf (stderr, "cannot prepare audio interface for use (%s)\n", |
| snd_strerror (err)); |
| exit (1); |
| } |
| // for (period = 0; period < 2; period++) |
| |
| /* for (period = 0; period < 2; period++) { |
| |
| err = snd_pcm_writei(playback_handle, (chirp+period*period_size), period_size); |
| if (err < 0) { |
| err = snd_pcm_writei(playback_handle, chirp, period_size); |
| if (err < 0) |
| { |
| printf("Initial write error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| } |
| if (err != period_size) { |
| /* if (err != period_size) |
| { |
| printf("Initial write error: written %i expected %li\n", err, period_size); |
| exit(EXIT_FAILURE); |
| } |
| }*/ |
| |
| // register capture callback |
| err = snd_async_add_pcm_handler(&chandler, capture_handle, async_capture_callback, &data); // fill by dummy &data |
| if (err < 0) { |
| /* err = snd_async_add_pcm_handler(&chandler, capture_handle, async_capture_callback, &data); // fill by dummy &data |
| if (err < 0) |
| { |
| printf("Unable to register async handler\n"); |
| exit(EXIT_FAILURE); |
| } |
| }*/ |
| |
| snd_pcm_link(capture_handle,playback_handle); //link capture and playback together |
| |
| //start sream |
| if ((err = snd_pcm_prepare (capture_handle)) < 0) { |
| /* if ((err = snd_pcm_prepare (capture_handle)) < 0) |
| { |
| fprintf (stderr, "cannot prepare audio interface for use (%s)\n", |
| snd_strerror (err)); |
| exit (1); |
| } |
| else printf("Capture device prepared...\n");*/ |
| |
| err = snd_pcm_start(capture_handle); |
| if (err < 0) { |
| err = snd_pcm_start(playback_handle); |
| if (err < 0) |
| { |
| printf("Start error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| } |
| else printf("Waiting for transmitt all samples\n"); |
| |
| //wait until all samples aren't transmitted |
| printf("Waiting for transmitt all samples\n"); |
| while(cperiod<10) { |
| sleep(1); |
| while ( snd_pcm_avail(capture_handle) < period_size ) |
| { |
| usleep(1000); |
| printf("."); |
| } |
| |
| //// stop audio?? |
| err = snd_pcm_drop(capture_handle); |
| if (err < 0) |
| { |
| printf("Stop error: %s\n", snd_strerror(err)); |
| exit(EXIT_FAILURE); |
| } |
| |
| |
| j=0; |
| for(i=0;i < SIGNAL_SAMPLES;i++){ |
| for (i=0;i < SIGNAL_SAMPLES;i++) |
| { |
| L_signal[i]=signal[j]; |
| R_signal[i]=signal[j+1]; |
| j+=2; |
| } |
| |
| // linear_windowed_chirp(L_signal,1000000, 1000); |
| |
| printf("\nData transmitted... \ncorrelating...\n"); |
| for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){ |
| printf("Data transmitted... \ncorrelating...\n"); |
| for (n=0; n < (SIGNAL_SAMPLES - chirp_size);n++) |
| { |
| l=0; |
| r=0; |
| for(m=CHIRP_OFFSET;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 |
| 423,23 → 415,28 |
|---|
| correlationr[n]=r; |
| } |
| |
| printf("\nSearching echos...\n"); |
| printf("Searching echos...\n"); |
| r=0; |
| l=0; |
| for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){ //najde nejvetsi korelace |
| if (l < correlationl[n]){ |
| for (n=0; n < (SIGNAL_SAMPLES - chirp_size);n++) //najde nejvetsi korelace |
| { |
| if (l < correlationl[n]) |
| { |
| delay[1] = n; |
| l = correlationl[n]; |
| } |
| if (r < correlationr[n]){ |
| if (r < correlationr[n]) |
| { |
| delay[2] = n; |
| r = correlationr[n]; |
| } |
| } |
| |
| printf("\nWriting output file...\n"); |
| out=fopen("/tmp/sonar.txt","w"); |
| j=0; |
| for(i=0;i<=100000;i++){ |
| for (i=0;i<=period_size;i++) |
| { |
| fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],L_signal[i],R_signal[i],correlationl[i], correlationr[i]); |
| j+=2; |
| } |