Go to most recent revision | Blame | Last modification | View Log | Download
///////////////////////////////////////////////////////////////////////////////////// A small demo of sonar.// Program allow distance measuring.// Uses cross-correlation algorithm to find echos//// Author: kaklik (kaklik@mlab.cz)/////////////////////////////////////////////////////////////////////////////////////#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>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 int verbose = 0; /* verbose flag */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 0unsigned int chirp_size;int period=0;int cperiod=0;int chirp[100000];short signal[1000000]; // record 6s of input samplesstatic 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;/* set the period time */err = 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;/* 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: *//* (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 ){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 ){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;}struct async_private_data{signed short *samples;snd_pcm_channel_area_t *areas;unsigned int period;};////// SIGNAL GENERATION STUFF/*int linear_chirp(int *pole, int delka_pole){ // vygeneruje linearni chirp a vzorky ulozi do polestatic 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 poleunsigned int linear_windowed_chirp ( unsigned int *pole, unsigned int delka_pole,unsigned int offset ){unsigned int maxval = ( 1 << ( snd_pcm_format_width ( format ) - 1 ) ) - 1;static const float f0 = 1000;static const float fmax = 7000;static const float Tw = 0.002;static float k;unsigned int n=0;double t;unsigned int perioda;k=2* ( fmax-f0 ) /Tw;perioda = rate*Tw;for ( n=0;n<=perioda;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 ) ) ) );}return ( perioda+offset );}// generate sine samples and storeint 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 pingunsigned 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 ){snd_pcm_t *handle = snd_async_handler_get_pcm ( ahandler );/* struct async_private_data *data = snd_async_handler_get_callback_private(ahandler);signed short *samples = data->samples;snd_pcm_channel_area_t *areas = data->areas;*/snd_pcm_sframes_t avail;int err;avail = snd_pcm_avail_update ( handle );while ( ( avail >= period_size ) && ( ( period*period_size ) < chirp_size ) ){err = snd_pcm_writei ( handle, ( chirp+period*period_size ), period_size );if ( err < 0 ){printf ( "Write error: %s\n", snd_strerror ( err ) );exit ( EXIT_FAILURE );}if ( err != period_size ){printf ( "Write error: written %i expected %li\n", err, period_size );exit ( EXIT_FAILURE );}avail = snd_pcm_avail_update ( handle );period++;}}static void async_capture_callback ( snd_async_handler_t *ahandler ){snd_pcm_t *handle = snd_async_handler_get_pcm ( ahandler );/* struct async_private_data *data = snd_async_handler_get_callback_private(ahandler);signed short *samples = data->samples;snd_pcm_channel_area_t *areas = data->areas;*/snd_pcm_sframes_t avail;int err;avail = snd_pcm_avail_update ( handle );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 ){printf ( "Read error: %s\n", snd_strerror ( err ) );exit ( EXIT_FAILURE );}if ( err != period_size ){printf ( "Read error: red %i expected %li\n", err, period_size );exit ( EXIT_FAILURE );}avail = snd_pcm_avail_update ( handle );cperiod++;}}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;signed short *frame; // pointer to array of samplesunsigned int chn;snd_pcm_channel_area_t *areas;struct async_private_data data;snd_async_handler_t *chandler, *phandler;int count;unsigned int i,j,m,n;unsigned int delay[10]; //store delay of signifed correlationlong int l,r; // store correlation at strict timelong int correlationl[SIGNAL_SAMPLES]; //array to store correlation curvelong int correlationr[SIGNAL_SAMPLES]; //array to store correlation curveint L_signal[SIGNAL_SAMPLES];int R_signal[SIGNAL_SAMPLES];FILE *out;snd_pcm_hw_params_alloca ( &hwparams );snd_pcm_sw_params_alloca ( &swparams );printf ( "Simple PC sonar ver. 000000001 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 );}/// generate ping patternchirp_size=linear_windowed_chirp ( chirp,1000000, CHIRP_OFFSET );/// register playback callbackerr = snd_async_add_pcm_handler ( &phandler, playback_handle, async_playback_callback, &data ); // fill by dummy &dataif ( err < 0 ){printf ( "Unable to register async handler\n" );exit ( EXIT_FAILURE );}for ( period = 0; period < 2; period++ ){err = snd_pcm_writei ( playback_handle, ( chirp+period*period_size ), period_size );if ( err < 0 ){printf ( "Initial write error: %s\n", snd_strerror ( err ) );exit ( EXIT_FAILURE );}if ( err != period_size ){printf ( "Initial write error: written %i expected %li\n", err, period_size );exit ( EXIT_FAILURE );}}// register capture callbackerr = snd_async_add_pcm_handler ( &chandler, capture_handle, async_capture_callback, &data ); // fill by dummy &dataif ( 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 sreamif ( ( err = snd_pcm_prepare ( capture_handle ) ) < 0 ){fprintf ( stderr, "cannot prepare audio interface for use (%s)\n",snd_strerror ( err ) );exit ( 1 );}err = snd_pcm_start ( capture_handle );if ( err < 0 ){printf ( "Start error: %s\n", snd_strerror ( err ) );exit ( EXIT_FAILURE );}//wait until all samples aren't transmittedprintf ( "Waiting for transmitt all samples\n" );while ( cperiod<10 ){sleep ( 1 );printf ( "." );}//// stop audio??j=0;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++ ){l=0;r=0;for ( m=CHIRP_OFFSET;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]=l;correlationr[n]=r;}printf ( "\nSearching echos...\n" );r=0;l=0;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] ){delay[2] = n;r = correlationr[n];}}out=fopen ( "./output.txt","w" );j=0;for ( i=0;i<=100000;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;}fclose ( out );printf ( "\nEcho zacina na: %d vzorku.\n", delay[1] );printf ( "Casove na: %f s\n", ( ( float ) delay[1]/rate ) );printf ( "vzdalenost: %f m\n", ( SOUND_SPEED* ( float ) delay[1]/rate ) );snd_pcm_close ( playback_handle );snd_pcm_close ( capture_handle );return 0;}