WebSVN – svnkaklik – Diff – /programy/C/ix86/echo/SW/sonar/src/sonar.c

 #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 = 96000;			/* stream rate */
-static unsigned int buffer_time = MAX_RANGE / SOUND_SPEED * 1e6;		/* ring buffer length in us */
+static unsigned int buffer_time = 2 * (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 unsigned int period_time = MAX_RANGE / SOUND_SPEED * 1e6;		/* period time in us */
 static int resample = 1;				/* enable alsa-lib resampling */
-#define SIGNAL_SAMPLES 100000
 unsigned int chirp_size;
-int period=0;
-int cperiod=0;
-short *chirp;
-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)
         return err;
+    }
     return 0;
+}
-struct async_private_data
-{
-    signed short *samples;
-    snd_pcm_channel_area_t *areas;
-    unsigned int period;
-};
 ////// SIGNAL GENERATION STUFF
 unsigned int linear_windowed_chirp(short *pole)
+{
     unsigned int maxval = (1 << (snd_pcm_format_width(format) - 1)) - 1;
-    static const float f0 = 1000;		//starting frequency
+    static const float f0 = 5000;		//starting frequency
-    static const float fmax = 7000;		//ending frequency
+    static const float fmax = 13000;		//ending frequency
-    static const float Tw = 0.002;
+    static const float Tw = 0.0015;
     static float k;
     unsigned int n=0;
     double t;
     unsigned int chirp_samples;		// number of samples per period
         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 (chirp_samples);
+}
-/////////// CALL BACK STUFF ///////////////////
-static void async_playback_callback(snd_async_handler_t *ahandler)
-{
-    snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
-    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);
-    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;
+    long int *correlationl, *correlationr;
-    snd_async_handler_t *chandler, *phandler;
+    int *L_signal, *R_signal;
-    int count;
+    short *chirp, *signal;
     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("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);
-// register playback callback
-/*    err = snd_async_add_pcm_handler(&phandler, playback_handle, async_playback_callback, &data); // fill by dummy &data
+/*    err = snd_pcm_link( capture_handle, playback_handle); //link capture and playback together
     if (err < 0)
+    {
-        printf("Unable to register async handler\n");
+        printf("Device linking error: %s\n", snd_strerror(err));
         exit(EXIT_FAILURE);
     }*/
+    correlationl = malloc(period_size * sizeof(long int)); //array to store correlation curve
+    correlationr = malloc(period_size * sizeof(long int)); //array to store correlation curve
+    L_signal = malloc(period_size * sizeof(int));
+    R_signal = malloc(period_size * sizeof(int));
+    chirp = calloc(2*period_size, sizeof(short));
+    signal = malloc(2*period_size * sizeof(short));
+// generate ping pattern
-//    for (period = 0; period < 2; period++)
+    chirp_size = linear_windowed_chirp(chirp);
     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)
-        {
-            printf("Initial write error: written %i expected %li\n", err, period_size);
-            exit(EXIT_FAILURE);
-        }*/
-// register capture callback
+//start sream
-/*    err = snd_async_add_pcm_handler(&chandler, capture_handle, async_capture_callback, &data); // fill by dummy &data
+    err = snd_pcm_start(playback_handle);
     if (err < 0)
+    {
-        printf("Unable to register async handler\n");
+        printf("Start error: %s\n", snd_strerror(err));
         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);
+    }
-    else printf("Capture device prepared...\n");*/
-    err = snd_pcm_start(playback_handle);
+    err = snd_pcm_start(capture_handle);
     if (err < 0)
+    {
         printf("Start error: %s\n", snd_strerror(err));
         exit(EXIT_FAILURE);
+    }
     else printf("Waiting for transmitt all samples\n");
+//--------------
-    while ( snd_pcm_avail(capture_handle) < period_size )
+    while ( snd_pcm_avail_update(capture_handle) < period_size)
+    {
         usleep(1000);
         printf(".");
+    }
+    err = snd_pcm_drop(playback_handle);
-    err = snd_pcm_drop(capture_handle);
+    err = snd_pcm_drain(capture_handle);
     if (err < 0)
+    {
         printf("Stop error: %s\n", snd_strerror(err));
         exit(EXIT_FAILURE);
+    }
+    err = snd_pcm_readi(capture_handle, signal, period_size);
+    if (err < 0)
+    {
+        printf("Read error: %s\n", snd_strerror(err));
+        exit(EXIT_FAILURE);
+    }
     j=0;
-    for (i=0;i < SIGNAL_SAMPLES;i++)
+    for (i=0;i < period_size;i++)		// separe inretleaved samples to two arrays
+    {
         L_signal[i]=signal[j];
         R_signal[i]=signal[j+1];
         j+=2;
+    }
-    printf("Data transmitted... \ncorrelating...\n");
+    printf("\nData transmitted \ncorrelating\n");
-    for (n=0; n < (SIGNAL_SAMPLES - chirp_size);n++)
+    for (n=0; n < (period_size - chirp_size - 1); n++)
+    {
         l=0;
         r=0;
         for (m=0;m < chirp_size;m++)
+        {
+        }
         correlationl[n]=l;
         correlationr[n]=r;
+    }
-    printf("Searching echos...\n");
+    printf("Searching echos\n");
     r=0;
     l=0;
-    for (n=0; n < (SIGNAL_SAMPLES - chirp_size);n++) 			//najde nejvetsi korelace
+    for (n=0; n < period_size;n++) 			//najde nejvetsi korelace
+    {
         if (l < correlationl[n])
+        {
             delay[1] = n;
             l = correlationl[n];
             delay[2] = n;
             r = correlationr[n];
+        }
+    }
-    printf("\nWriting output file...\n");
+    printf("Writing output file\n");
     out=fopen("/tmp/sonar.txt","w");
     j=0;
-    for (i=0;i<=period_size;i++)
+    for (i=0; i <= (period_size -1); 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("Echo 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);

Subversion Repositories svnkaklik

(root)/programy/C/ix86/echo/SW/sonar/src/sonar.c @ 562 – Rev 561 → 562