Subversion Repositories svnkaklik

///////////////////////////////////////////////////////////////////////////////////

//                        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	0

unsigned int chirp_size;

int period=0;

int cperiod=0;

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 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 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 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 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 )

{

	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 samples

	unsigned 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 correlation

	long int l,r;  // store correlation at strict time

	long int correlationl[SIGNAL_SAMPLES]; //array to store correlation curve

	long int correlationr[SIGNAL_SAMPLES]; //array to store correlation curve

	int 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 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;

	}

	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 device

	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 )

	{

		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 pattern

	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 )

	{

		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 callback

	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 )

	{

		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 transmitted

	printf ( "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 channel

			r += 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;

}