Rev Author Line No. Line
3298 kaklik 1 ///////////////////////////////////////////////////////////////////////////////////
2 // A small demo of sonar.
3 // Program allow distance measuring.
4 // Uses cross-correlation algorithm to find echos
5 //
6 // Author: kaklik (kaklik@mlab.cz)
7 //$Id:$
8 ///////////////////////////////////////////////////////////////////////////////////
9  
10 #include <stdio.h>
11 #include <stdlib.h>
12 #include <math.h>
13 #include <SDL.h>
14 #include <alsa/asoundlib.h>
15  
16 #define SOUND_SPEED 340.0 // sound speed in air in metrs per second
17 #define MAX_RANGE 5.0 // maximal working radius in meters
18 #define Xl -0.1 // microphones position
19 #define Xr 0.1
20  
21 // Alsa settings
22 static char *alsa_device = "plughw:0,0";
23 static snd_pcm_format_t alsa_format = SND_PCM_FORMAT_S16; /* sample format */
24 static unsigned int alsa_rate = 96000; /* stream rate */
25 static unsigned int alsa_buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6); /* ring buffer length in us */
26 static unsigned int alsa_period_time = MAX_RANGE / SOUND_SPEED * 1e6; /* period time in us */
27 static int alsa_resample = 1;
28  
29 #define V2D SOUND_SPEED/alsa_rate // count of values to distance
30  
31 // Alsa driver
32  
33 // Hasn't so pretty error messages, but can really cut off count of the lines
34 #define ALSA_ERR(a) { \
35 int err; \
36 err = a; \
37 if (err < 0) { \
38 printf("alsa: " #a /* stringify a */ ": %s\n", snd_strerror(err)); \
39 return err; \
40 } \
41 }
42  
43 static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels, unsigned int rate,
44 int resample, unsigned int buffer_time, snd_pcm_sframes_t *buffer_size,
45 unsigned int period_time, snd_pcm_sframes_t *period_size, snd_pcm_format_t format)
46 {
47 unsigned int rrate;
48 snd_pcm_uframes_t size;
49 int dir;
50  
51 /* choose all parameters */
52 ALSA_ERR(snd_pcm_hw_params_any(handle, params));
53 /* set hardware resampling */
54 ALSA_ERR(snd_pcm_hw_params_set_rate_resample(handle, params, resample));
55 /* set the interleaved read/write format */
56 ALSA_ERR(snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED));
57 /* set the sample format */
58 ALSA_ERR(snd_pcm_hw_params_set_format(handle, params, format));
59 /* set the count of channels */
60 ALSA_ERR(snd_pcm_hw_params_set_channels(handle, params, channels));
61  
62 /* set the stream rate */
63 rrate = rate;
64 ALSA_ERR(snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0));
65 if (rrate != rate) {
66 printf("alsa: rate doesn't match (requested %iHz, get %iHz)\n", rate, rrate);
67 return -EINVAL;
68 } else {
69 printf("alsa: rate set to %i Hz\n", rate);
70 }
71  
72 /* set the buffer time */
73 ALSA_ERR(snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir));
74 ALSA_ERR(snd_pcm_hw_params_get_buffer_size(params, &size));
75 *buffer_size = size;
76 printf("alsa: bufffer size set to: %d requested buffer time: %ld \n", (int) *buffer_size,
77 (long) buffer_time);
78  
79 // set the period time
80 ALSA_ERR(snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir));
81 ALSA_ERR(snd_pcm_hw_params_get_period_size(params, &size, &dir));
82 *period_size = size;
83 printf("alsa: period size set to: %d requested period time: %ld \n", (int) *period_size,
84 (long) period_time);
85  
86 /* write the parameters to device */
87 ALSA_ERR(snd_pcm_hw_params(handle, params));
88  
89 return 0;
90 }
91  
92 static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams, unsigned int buffer_size)
93 {
94 /* get the current swparams */
95 ALSA_ERR(snd_pcm_sw_params_current(handle, swparams));
96  
97 // start the transfer when the buffer is almost full: never for our case
98 ALSA_ERR(snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size));
99 ALSA_ERR(snd_pcm_sw_params_set_period_event(handle, swparams, 1));
100  
101 /* write the parameters to the playback device */
102 ALSA_ERR(snd_pcm_sw_params(handle, swparams));
103  
104 return 0;
105 }
106  
107 unsigned int linear_windowed_chirp(short *pole) // generate the ping signal
108 {
109 unsigned int maxval = (1 << (snd_pcm_format_width(alsa_format) - 1)) - 1;
110  
111 static const float f0 = 5000; // starting frequency
112 static const float fmax = 10000; // ending frequency
113 static const float Tw = 0.0015; // time width of ping in seconds
114 static float k;
115  
116 unsigned int n=0;
117 double t;
118 unsigned int chirp_samples; //number of samples per period
119  
120 k=2*(fmax-f0)/Tw;
121 chirp_samples = ceil(alsa_rate*Tw); // compute size of ping sinal in samples
122  
123 for (n=0;n<=chirp_samples;n++)
124 {
125 t = (double) n / (double)alsa_rate;
126 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))) ); // signal generation formula
127 }
128 return (chirp_samples); // return count of samples in ping
129 }
130  
131 int main (int argc, char *argv[]) {
132 snd_pcm_t *playback_handle, *capture_handle;
133 snd_pcm_hw_params_t *hwparams;
134 snd_pcm_sw_params_t *swparams;
135 snd_pcm_sframes_t buffer_size; // size of buffer at sound card
136 snd_pcm_sframes_t period_size; // samples per frame
137  
138 int err, j, l, i, r;
139 unsigned int n, m;
140  
141 printf("Simple PC sonar $Rev:$ starting work.. \n");
142  
143 printf("Initializing ALSA.. \r");
144 fflush(stdout);
145  
146 snd_pcm_hw_params_alloca(&hwparams); // allocation of soundcard parameters registers
147 snd_pcm_sw_params_alloca(&swparams);
148  
149 ALSA_ERR(snd_pcm_open(&playback_handle, alsa_device, SND_PCM_STREAM_PLAYBACK, 0));
150 ALSA_ERR(set_hwparams(playback_handle, hwparams, 1, alsa_rate, alsa_resample,
151 alsa_buffer_time, &buffer_size, alsa_period_time, &period_size, alsa_format));
152 ALSA_ERR(set_swparams(playback_handle, swparams, buffer_size));
153  
154 ALSA_ERR(snd_pcm_open(&capture_handle, alsa_device, SND_PCM_STREAM_CAPTURE, 0));
155 ALSA_ERR(set_hwparams(capture_handle, hwparams, 2, alsa_rate, alsa_resample,
156 alsa_buffer_time, &buffer_size, alsa_period_time, &period_size, alsa_format));
157 ALSA_ERR(set_swparams(capture_handle, swparams, buffer_size));
158  
159 printf("Initializing ALSA.. done!\n");
160  
161 short *chirp = calloc(2*period_size, sizeof(short));
162 short *signal = malloc(2*period_size * sizeof(short));
163 short *signal_l = malloc(period_size * sizeof(short));
164 short *signal_r = malloc(period_size * sizeof(short));
165 long int *correlation_l = malloc(period_size * sizeof(long int)); // arrays to store correlation curve
166 long int *correlation_r = malloc(period_size * sizeof(long int));
167 float *echo_map = malloc(period_size * period_size * sizeof(float));
168 unsigned char *echo_map_ = malloc(400 * 400 * sizeof(unsigned char));
169  
170 if (!chirp || !signal) {
171 printf("Can't allocate buffers!\n");
172 }
173  
174 printf("Generating ping signal.. \r");
175 fflush(stdout);
176  
177 unsigned int chirp_size = linear_windowed_chirp(chirp);
178  
179 printf("Generating ping signal.. done!\n");
180  
181 SDL_Surface *sdl_screen;
182 {
183 SDL_Init(SDL_INIT_VIDEO);
184 sdl_screen = SDL_SetVideoMode(400, 400, 32, SDL_SWSURFACE);
185 }
186  
187 while (1) {
188 ALSA_ERR(snd_pcm_writei(playback_handle, chirp, period_size));
189 ALSA_ERR(snd_pcm_start(playback_handle));
190  
191 ALSA_ERR(snd_pcm_start(capture_handle));
192  
193 while (snd_pcm_avail_update(capture_handle) < period_size) {
194 usleep(1000);
195 }
196  
197 ALSA_ERR(snd_pcm_drop(playback_handle));
198 ALSA_ERR(snd_pcm_drain(capture_handle));
199  
200 ALSA_ERR(snd_pcm_readi(capture_handle, signal, period_size));
201  
202 for (i = 0; i < period_size * 2; i += 2) {
203 signal_l[i >> 1] = signal[i];
204 signal_r[i >> 1] = signal[i + 1];
205 }
206  
207 {
208 printf("Chirp transmitted\nCorrelating\n");
209 for (n = 0; n < (period_size - chirp_size - 1); n++) {
210 l = 0;
211 r = 0;
212  
213 for (m = 0; m < chirp_size; m++) {
214 l += chirp[m] * signal_l[m + n];// correlate with left channel
215 r += chirp[m] * signal_r[m + n];// correlate with right channel
216 }
217  
218 correlation_l[n] = abs(l);
219 correlation_r[n] = abs(r);
220 }
221  
222 for (i = 0; i < period_size * period_size; i += 1)
223 echo_map[i] = 0.0f;
224 for (i = 0; i < 400 * 400; i += 1)
225 echo_map_[i] = 0;
226  
227 for (i = 0; i < period_size; i += 1) {
228 double f = V2D * i;
229  
230 for (j = 0; j < period_size; j += 1) {
231 double g = V2D * j;
232 double a, b;
233  
234 /*
235 a=(2*f*g*Xl+f*f*Xr+Xl*(g*g+(Xl-Xr)*Xr))/(2*g*Xl+2*f*Xr);
236 b=(g*g*Xl-2*f*g*Xr+Xr*(f*f+Xl*(-Xl+Xr)))/(2*g*Xl-2*f*Xr);
237 */
238  
239 a = f - sqrt(pow(g * g * Xl - Xr * (f * f + Xl *
240 (-Xl + Xr)), 2) / pow(g * Xl - f * Xr, 2)) / 2.0;
241 b = g - sqrt(pow(g * g * Xl - Xr * (f * f + Xl *
242 (-Xl + Xr)), 2) / pow(g * Xl - f * Xr, 2)) / 2.0;
243  
244 if (((Xr-Xl)<=a+b) && (b<=a+(Xr-Xl)) && (a<=b+(Xr-Xl))) {
245 /*
246 float x = (f*((f-g)*g + Xl*Xl)-b*Xr*Xr)/(2*f*Xl-2*g*Xr);
247 float y = sqrt(((-g*g+Xl*Xl)*(f-Xr)*(f-g+Xl-Xr)*(f+Xr)
248 *(f-g-Xl+Xr))/(4*(f*Xl-g*Xr)*(f*Xl-g*Xr)));
249 */
250  
251 errno = 0;
252  
253 float x = (f * f * g - g * Xl * Xl + f
254 * (- g * g + Xr * Xr))
255 / (-2 * g * Xl + 2 * f * Xr);
256  
257 float y = sqrt(-((g * g * (f * f - Xl * Xl)
258 * (f * f - 2 * f * g + g * g
259 - (Xl - Xr) * (Xl - Xr)) * (g * g - Xr * Xr))
260 / pow(g * Xl - f * Xr, 2)))/(2.0 * g);
261  
262 /*
263 if (x < 0)
264 printf("%f\n", x);
265 */
266  
267 if ((x == x) && (y == y) && (errno != EDOM)) // check for NaN (NaN == NaN is false)
268 echo_map[period_size * ((int) (y / ((float) V2D)))
269 + ((int) (x / ((float) V2D))
270 + (period_size/2))] =
271 sqrt(correlation_l[i]/2)*sqrt(correlation_r[j]/2);
272 }
273 }
274 }
275  
276 for (i = 0; i < period_size; i += 1) {
277 for (j = 0; j < period_size; j += 1) {
278 int y = i * 400 / period_size;
279 int x = j * 400 / period_size;
280  
281 Uint8 green = echo_map[i*period_size+j]/(2000000000/255);
282 Uint8 *pointer = &echo_map_[y*400+x];
283  
284 if (green > *pointer)
285 *pointer = green;
286 }
287 }
288  
289  
290 for (i = 0; i < 400; i += 1) {
291 for (j = 0; j < 400; j += 1) {
292 Uint32* bufp = (Uint32 *)sdl_screen->pixels + i*sdl_screen->pitch/4 + j;
293 *bufp = SDL_MapRGB(sdl_screen->format, 0, echo_map_[i*400+j], 0);
294 }
295 }
296 SDL_UpdateRect(sdl_screen, 0, 0, 400, 400);
297  
298 SDL_Event sdl_event;
299 while (SDL_PollEvent(&sdl_event)) {
300 if (sdl_event.type == SDL_QUIT)
301 return 0;
302 }
303  
304 /*
305 FILE *out = fopen("output", "w");
306 for (i = 0; i <= (period_size - 1); i++) {
307 fprintf(out, "%2.3f %6d %6d %9ld %9ld\n", (float)i*V2D, signal_l[i], signal_r[i], correlation_l[i], correlation_r[i]);
308 }
309 fclose(out);
310 */
311 }
312  
313 ALSA_ERR(snd_pcm_prepare(playback_handle));
314 ALSA_ERR(snd_pcm_prepare(capture_handle));
315 }
316  
317 return 0;
318 }