WebSVN – svnkaklik – Diff – /programy/C/ix86/sound/sonar.c

 #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 channels = 1;			/* count of channels */
 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 */
 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,
+static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels)
-			snd_pcm_hw_params_t *params,
-			snd_pcm_access_t access)
+{
 	unsigned int rrate;
 	snd_pcm_uframes_t size;
 	int err, dir;
 	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, access);
+	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 */
   k=2*(fmax-f0)/Tw;
   perioda = rate*Tw;
   for(n=0;n<=perioda;n++){
      t = (double) n/ (double)rate;
-     pole[n+offset] = (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))) );
+     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
 unsigned int n;
 double t;
   for(n=0;n < delka_pole;n++){
     t = 440.0 * (double) n/ (double)rate;
-    pole[n] = (short) round(maxval*sin(2*M_PI*t));
+    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 n;
 double t;
   for(n=0;n < delka_pole;n++){
     t = frequency * (double) n/ (double)rate;
-    pole[n] = (short) round(maxval*sin(2*M_PI*t));
+    pole[n] = (short) floor(maxval*sin(2*M_PI*t));
+  }
+}
 /////////// CALL BACK STUFF ///////////////////
 static void async_playback_callback(snd_async_handler_t *ahandler)
 	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
-  	double r;
+  	double l,r;  // store correlation at strict time
+	long int correlationl[100000]; //array to store correlation curve
-	double correlation[1000000]; //array to store correlation curve
+	long int correlationr[100000]; //array to store correlation curve
 	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, SND_PCM_ACCESS_RW_INTERLEAVED)) < 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));
 	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, SND_PCM_ACCESS_RW_INTERLEAVED)) < 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, 300);
+        chirp_size=linear_windowed_chirp(chirp,1000000, 200);
 /// 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");
 	while(cperiod<10) {
 		sleep(1);
 		printf(".");
+	}
 	printf("\nData transmitted... \ncorrelating...\n");
+	i=0;
-	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){			//spocita korelaci pro mozna spozdeni
+	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){			//we have interleaved data we must have 2 saples step
+	  l=0;
+          r=0;
-	  r=0;
+	  j=0;
+	  for(m=0;m < chirp_size;m++)
+          {
-	  for(m=chirp_size;m > 0;m--) r += chirp[m]*signal[m+n];	// reverse echo patern
+            l += chirp[m]*signal[i+j];	// correlate with left channel
+            r += chirp[m]*signal[i+j+1];	// correlate with right channel
+	    j+=2;
+          }
+	  correlationl[n]=l;
-	  correlation[n]=abs(r);
+	  correlationr[n]=r;
+	  i+=2;
+	}
-	printf("\nFinding echos...\n");
+/*	printf("\nFinding echos...\n");
 	r=0;
+	l=0;
 	for(n=0; n < (SIGNAL_SAMPLES - chirp_size);n++){			//najde nejvetsi korelace
-	  if (r < correlation[n]){
+	  if (l < correlationl[n]){
 	  delay[1] = n;
-	  r = correlation[n];
+	  l = correlationl[n];
+	  }
+	  if (r < correlationr[n]){
+	  delay[1] = n;
+	  r = correlationr[n];
+	  }
-	}
+	}*/
 	out=fopen("./output.txt","w");
+  j=0;
+  for(i=0;i<=100000;i++){
-	for(i=0;i<=100000;i++) fprintf(out,"%6d	%6d %6d %6.2f\n",i,chirp[i],signal[i],correlation[i]);
+    fprintf(out,"%6d %6d %6d %6d %9ld %9ld\n",i,chirp[i],signal[j],signal[j+1],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;
+}

Subversion Repositories svnkaklik

(root)/programy/C/ix86/sound/sonar.c @ 540 – Rev 539 → 540