Rev 1086 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1085 | pj | 1 | // FFT Part |
2 | /* |
||
3 | |||
4 | FFTPlay 1.0 |
||
5 | ----------- |
||
6 | |||
7 | This application reads data from the audio microphone and then put it |
||
8 | to the screen in a graphical oscilloscope-like form (both standard Hartik |
||
9 | SB driver and Alsa driver can be used). |
||
10 | |||
11 | The application also calculate a FFT on the latest values, and displays |
||
12 | the power spectrum in tho ways, one like an equalizator, and the other in a |
||
13 | 2D form. |
||
14 | |||
15 | A resolution of 1024x768, 64K colors is used. |
||
16 | |||
17 | The task set is composed by these tasks and functions: |
||
18 | |||
19 | Self buffering functions (SB only) |
||
20 | ---------------------------------- |
||
21 | this function takes the samples read by the mic and then makes a window |
||
22 | with the last WINDATA_NSAMPLES samples. The window is then put in |
||
23 | the CAB windata. |
||
24 | |||
25 | Task raw and task mixer (ALSA only) |
||
26 | ----------------------------------- |
||
27 | These tasks are used with the Alsa driver; because it doesn't support the |
||
28 | self-buffering mode, we have to do a forever cycle in witch we have to read |
||
29 | all the data. Then the data are managed like the self-buffering functions of |
||
30 | the SB driver or sent to the mixer task (with a STREAM mailbox) which makes |
||
31 | the window. |
||
32 | (This approach is not good for realtime...) |
||
33 | |||
34 | Task wave |
||
35 | --------- |
||
36 | This task read the last window and then put it on the screen in a wave form |
||
37 | on the top of the screen. |
||
38 | The task's period is set to 40 ms (25 fps). |
||
39 | |||
40 | Task fft |
||
41 | -------- |
||
42 | This task read the last window and then it computes the FFT. |
||
43 | With the FFT data it computes the power spectrum, whitch is sent to the |
||
44 | CAB pwrdata. |
||
45 | The task's period is set to 10 ms (good for the 2D story task). |
||
46 | This task is the only Hard Task. |
||
47 | |||
48 | Task equ |
||
49 | -------- |
||
50 | This task read the last power spectrum and displays it in a graphical |
||
51 | form, like a hi-fi equalizator. |
||
52 | The Histograms can be white or coloured like the equ2D task (see EQU_SHADE) |
||
53 | The task's period is set to 40 ms (25 fps). |
||
54 | |||
55 | Task equ2D |
||
56 | ---------- |
||
57 | This task read the lasf power spectrum and displays it in a graphical |
||
58 | one-line form. Each pixel is a power coefficient, and its colour shade |
||
59 | from black (no power) to red (high power) passing through green and blue. |
||
60 | The task display the last EQU2D_WIDTH power lines. |
||
61 | The task's period is set to 10 ms (good for the 2D story task). |
||
62 | |||
63 | **************************************************************************** |
||
64 | TASK LOAD |
||
65 | **************************************************************************** |
||
66 | |||
67 | period wcet |
||
68 | task tick (ms) us % |
||
69 | -------------------------------------------------------- |
||
70 | sound driver 24->3 12->1.5 200 0.016->0.133 |
||
71 | wave 80 40 11500 0.2875 |
||
72 | fft 20 10 3000 0.3000 |
||
73 | equ 80 40 7000 0.1750 |
||
74 | equ2D 20 10 500 0.0500 |
||
75 | ------------- |
||
76 | 0.812 (last 4) |
||
77 | */ |
||
78 | |||
79 | #include "demo.h" |
||
80 | //#include <ll/ll.h> |
||
81 | |||
82 | //#include <kernel/types.h> |
||
83 | #include <kernel/model.h> |
||
84 | #include <kernel/func.h> |
||
85 | |||
86 | #include <modules/cabs.h> |
||
87 | |||
88 | //#include <string.h> |
||
89 | //#include <stdlib.h> |
||
90 | #include <semaphore.h> |
||
91 | |||
92 | //#include <drivers/keyb.h> |
||
93 | //#include <drivers/crtwin.h> |
||
94 | #include <drivers/glib.h> |
||
95 | #include <drivers/sound.h> |
||
1144 | pj | 96 | #include <rfftw.h> |
1085 | pj | 97 | |
98 | /* CAB ports... */ |
||
99 | CAB cab_windata; /* a window on the last WINDATA_DIM samples */ |
||
100 | CAB cab_pwrdata; /* the last power spectrum */ |
||
101 | |||
102 | /* for the cab_windata */ |
||
103 | typedef struct { |
||
104 | int start; |
||
105 | SAMPLE sample[WINDATA_NSAMPLES]; |
||
106 | } window; |
||
107 | |||
108 | /* for the cab_pwrdata */ |
||
109 | typedef struct { |
||
110 | fftw_real p[PWR_NSAMPLES]; |
||
111 | } power; |
||
112 | |||
113 | |||
114 | // win is global... because is used by raw_infun... |
||
115 | window win; |
||
116 | |||
117 | /* |
||
118 | This is the self-buffering function: read the samples and put their mean |
||
119 | value in a CAB |
||
120 | */ |
||
121 | int raw_infun(void *b) |
||
122 | { |
||
123 | int i; |
||
124 | char *w; |
||
125 | SAMPLE *audiobuf = (SAMPLE *)b; |
||
126 | |||
127 | for (i=0; i<rawdata_nsamples/2; i++) { |
||
128 | win.sample[win.start] = audiobuf[i]; |
||
129 | win.start = (win.start+1) % WINDATA_NSAMPLES; |
||
130 | } |
||
131 | |||
132 | w = cab_reserve(cab_windata); |
||
133 | memcpy(w, &win, sizeof(window)); |
||
134 | cab_putmes(cab_windata,w); |
||
135 | |||
136 | #if defined(NO_GRX) |
||
137 | cprintf("X"); //"XXX%d\n",win.sample[win.start]); |
||
138 | #endif |
||
139 | return 0; |
||
140 | } |
||
141 | |||
142 | |||
143 | |||
144 | void init_rawdata() |
||
145 | { |
||
146 | int i; |
||
147 | char *w; |
||
148 | |||
149 | win.start = 0; |
||
150 | for (i=0; i<WINDATA_NSAMPLES; i++) |
||
151 | win.sample[i] = 0; |
||
152 | |||
153 | w = cab_reserve(cab_windata); |
||
154 | memcpy(w, &win, sizeof(window)); |
||
155 | cab_putmes(cab_windata,w); |
||
156 | } |
||
157 | |||
158 | |||
159 | |||
160 | |||
161 | |||
162 | TASK wave_task() |
||
163 | { |
||
164 | window *p; |
||
165 | int x,y; |
||
166 | int s; |
||
167 | |||
168 | while(1) |
||
169 | { |
||
170 | p = (window *)cab_getmes(cab_windata); |
||
171 | |||
172 | /* let's print the wave */ |
||
173 | mutex_lock(&mutex); |
||
174 | for(x = WAVE_X, s = p->start; |
||
175 | x < WAVE_X+WAVE_NSAMPLES; |
||
176 | x++, s = (s+1)%WINDATA_NSAMPLES ) |
||
177 | { |
||
178 | y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE; |
||
179 | SHORT_CRITICAL_SECTIONS(x); |
||
180 | grx_plot(x,y,white); |
||
181 | } |
||
182 | mutex_unlock(&mutex); |
||
183 | |||
184 | task_endcycle(); |
||
185 | |||
186 | /* let's erase the wave */ |
||
187 | mutex_lock(&mutex); |
||
188 | for(x = WAVE_X, s = p->start; |
||
189 | x < WAVE_X+WAVE_NSAMPLES; |
||
190 | x++, s = (s+1)%WINDATA_NSAMPLES ) |
||
191 | { |
||
192 | y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE; |
||
193 | SHORT_CRITICAL_SECTIONS(x); |
||
194 | grx_plot(x,y,black); |
||
195 | } |
||
196 | mutex_unlock(&mutex); |
||
197 | |||
198 | cab_unget(cab_windata,(char *)p); |
||
199 | |||
200 | } |
||
201 | } |
||
202 | |||
203 | |||
204 | rfftw_plan plan; |
||
205 | |||
206 | void fft_close(void *arg) |
||
207 | { |
||
208 | rfftw_destroy_plan(plan); |
||
209 | } |
||
210 | |||
211 | |||
212 | TASK fft_task() |
||
213 | { |
||
214 | fftw_real in[FFT_NSAMPLES], out[FFT_NSAMPLES]; |
||
215 | power power_spectrum; |
||
216 | |||
217 | #if defined(NO_GRX) |
||
218 | fftw_real max = 0.0; |
||
219 | #endif |
||
220 | |||
221 | char *m; |
||
222 | |||
223 | int k, i; |
||
224 | |||
225 | window *p; |
||
226 | |||
227 | plan = rfftw_create_plan(FFT_NSAMPLES, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE); |
||
228 | |||
229 | sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT); |
||
230 | |||
231 | while(1) |
||
232 | { |
||
233 | /* Let's prepare the intput FFT data */ |
||
234 | p = (window *)cab_getmes(cab_windata); |
||
235 | |||
236 | for (k = 0, i = p->start; |
||
237 | k < FFT_NSAMPLES; |
||
238 | k++, i = (i+1)%WINDATA_NSAMPLES) |
||
239 | in[k] = p->sample[i]/FFT_SCALE; |
||
240 | |||
241 | cab_unget(cab_windata,(char *)p); |
||
242 | |||
243 | /* zero-padding if needed */ |
||
244 | for (k=WINDATA_NSAMPLES; k < FFT_NSAMPLES; k++) |
||
245 | in[k] = 0.0; |
||
246 | |||
247 | rfftw_one(plan, in, out); |
||
248 | |||
249 | /* power spectrum computation */ |
||
250 | power_spectrum.p[0] = out[0]*out[0]; /* DC component */ |
||
251 | for (k = 1; k < PWR_NSAMPLES; ++k) /* (k < N/2 rounded up) */ |
||
252 | power_spectrum.p[k] = out[k]*out[k] + out[FFT_NSAMPLES-k]*out[FFT_NSAMPLES-k]; |
||
253 | if (FFT_NSAMPLES % 2 == 0) /* N is even */ |
||
254 | power_spectrum.p[FFT_NSAMPLES/2] = out[FFT_NSAMPLES/2]*out[FFT_NSAMPLES/2]; /* Nyquist freq. */ |
||
255 | |||
256 | m = cab_reserve(cab_pwrdata); |
||
257 | memcpy(m, &power_spectrum, sizeof(power)); |
||
258 | cab_putmes(cab_pwrdata,m); |
||
259 | |||
260 | #if defined(NO_GRX) |
||
261 | max = 0.0; |
||
262 | for (k=0; k<PWR_NSAMPLES; k++) |
||
263 | if (power_spectrum.p[k] > max) |
||
264 | max = power_spectrum.p[k]; |
||
265 | |||
266 | //cprintf("%f %f\n",max,(max / EQU_SCALE) ); |
||
267 | #endif |
||
268 | |||
269 | task_endcycle(); |
||
270 | |||
271 | } |
||
272 | } |
||
273 | |||
274 | /* structure is like the wave task... */ |
||
275 | TASK equ_task() |
||
276 | { |
||
277 | power *p; |
||
278 | |||
279 | int x[PWR_NSAMPLES]; |
||
280 | int y; |
||
281 | int s; |
||
282 | |||
283 | int r,g,b; |
||
284 | |||
285 | while(1) |
||
286 | { |
||
287 | p = (power *)cab_getmes(cab_pwrdata); |
||
288 | |||
289 | /* print the lines */ |
||
290 | mutex_lock(&mutex); |
||
291 | for(y = EQU_Y, s = 0; |
||
292 | s < EQU_NSAMPLES; |
||
293 | y++, s++ ) |
||
294 | { |
||
295 | x[s] = (int)(p->p[s] / EQU_SCALE); |
||
296 | |||
297 | if (x[s] > EQU_HEIGHT) |
||
298 | x[s] = EQU_HEIGHT; |
||
299 | |||
300 | x[s] = EQU_X - x[s]; |
||
301 | |||
302 | |||
303 | #if defined(EQU_SHADE) |
||
304 | |||
305 | /* like the task equ2d... */ |
||
306 | r = (int)(p->p[s] / EQU2D_SCALE); |
||
307 | if (r > EQU2D_CLIP) |
||
308 | r = EQU2D_CLIP; |
||
309 | |||
310 | if (r< 64) g = r * 4; |
||
311 | else if (r<128) g = (128-r) * 4; |
||
312 | else g = 0; |
||
313 | |||
314 | if (r<128) b = 0; |
||
315 | else if (r<192) b = (r-128) * 4; |
||
316 | else b = (256-r) * 4; |
||
317 | |||
318 | SHORT_CRITICAL_SECTIONS(y); |
||
319 | grx_line(EQU_X,y,x[s],y,rgb16(r,g,b)); |
||
320 | #else |
||
321 | SHORT_CRITICAL_SECTIONS(y); |
||
322 | grx_line(EQU_X,y,x[s],y,white); |
||
323 | #endif |
||
324 | } |
||
325 | mutex_unlock(&mutex); |
||
326 | |||
327 | task_endcycle(); |
||
328 | |||
329 | /* erase the lines... */ |
||
330 | mutex_lock(&mutex); |
||
331 | for(y = EQU_Y, s = 0; |
||
332 | s < EQU_NSAMPLES; |
||
333 | y++, s++ ) |
||
334 | { |
||
335 | SHORT_CRITICAL_SECTIONS(y); |
||
336 | grx_line(EQU_X,y,x[s],y,black); |
||
337 | } |
||
338 | mutex_unlock(&mutex); |
||
339 | |||
340 | cab_unget(cab_pwrdata,(char *)p); |
||
341 | |||
342 | } |
||
343 | } |
||
344 | |||
345 | TASK equ2d_task() |
||
346 | { |
||
347 | power *p; |
||
348 | |||
349 | int pwrint; |
||
350 | |||
351 | int x = 0; |
||
352 | |||
353 | int y,s; |
||
354 | |||
355 | int r,g,b; |
||
356 | |||
357 | while(1) |
||
358 | { |
||
359 | |||
360 | p = (power *)cab_getmes(cab_pwrdata); |
||
361 | |||
362 | /* print the line */ |
||
363 | mutex_lock(&mutex); |
||
364 | |||
365 | for(y = EQU2D_Y, s = 0; |
||
366 | s < EQU2D_NSAMPLES; |
||
367 | y++, s++ ) |
||
368 | { |
||
369 | pwrint = (int)(p->p[s] / EQU2D_SCALE); |
||
370 | |||
371 | if (pwrint > EQU2D_CLIP) |
||
372 | pwrint = EQU2D_CLIP; |
||
373 | |||
374 | r = pwrint; |
||
375 | |||
376 | if (pwrint< 64) g = pwrint * 4; |
||
377 | else if (pwrint<128) g = (128-pwrint) * 4; |
||
378 | else g = 0; |
||
379 | |||
380 | if (pwrint<128) b = 0; |
||
381 | else if (pwrint<192) b = (pwrint-128) * 4; |
||
382 | else b = (256-pwrint) * 4; |
||
383 | |||
384 | SHORT_CRITICAL_SECTIONS(y); |
||
385 | grx_plot(EQU2D_X+x,y,rgb16(r,g,b)); |
||
386 | } |
||
387 | |||
388 | x = (x+1) % EQU2D_WIDTH; |
||
389 | grx_line(EQU2D_X+x,EQU2D_Y,EQU2D_X+x,EQU2D_Y+EQU2D_NSAMPLES-1,white); |
||
390 | |||
391 | mutex_unlock(&mutex); |
||
392 | |||
393 | cab_unget(cab_pwrdata,(char *)p); |
||
394 | |||
395 | task_endcycle(); |
||
396 | } |
||
397 | } |
||
398 | |||
399 | |||
400 | void init_fftplay(int freq) |
||
401 | { |
||
402 | SOFT_TASK_MODEL m3, m4, m5, m6; |
||
403 | |||
404 | PID p3,p4,p5,p6; |
||
405 | |||
406 | cab_windata = cab_create("windata", sizeof(window), 5); |
||
407 | cab_pwrdata = cab_create("pwr", sizeof(power), 5); |
||
408 | |||
409 | /* Init the sound lib */ |
||
410 | sound_init((rawdata_nsamples * sizeof(SAMPLE)), NULL); |
||
411 | sound_info(); |
||
412 | |||
413 | /* Init the data used by the raw_infun */ |
||
414 | init_rawdata(); |
||
415 | |||
416 | /* Start the self-buffering sampling operation */ |
||
417 | sound_setfun(raw_infun, (int (*)(void *))-1); |
||
418 | sound_sample(NULL, freq, 0, DMA_OP | PCM16 | MYFUN, NULL); |
||
419 | |||
420 | soft_task_default_model(m3); |
||
421 | soft_task_def_level(m3,1); |
||
422 | soft_task_def_period(m3, PERIOD_WAVE); |
||
423 | soft_task_def_met(m3, WCET_WAVE); |
||
424 | soft_task_def_ctrl_jet(m3); |
||
425 | soft_task_def_group(m3, 1); |
||
426 | p3 = task_create("wave", wave_task, &m3, NULL); |
||
427 | if (p3 == -1) { |
||
428 | grx_close(); |
||
429 | perror("FFTPlay: Could not create task <wave>\n"); |
||
430 | ll_abort(54); |
||
431 | sys_end(); |
||
432 | } |
||
433 | |||
434 | soft_task_default_model(m4); |
||
435 | soft_task_def_level(m4,1); |
||
436 | soft_task_def_period(m4, PERIOD_FFT); |
||
437 | soft_task_def_met(m4, WCET_FFT); |
||
438 | soft_task_def_group(m4, 1); |
||
439 | soft_task_def_stack(m4,32*1024); |
||
440 | soft_task_def_usemath(m4); |
||
441 | soft_task_def_ctrl_jet(m4); |
||
442 | p4 = task_create("fft", fft_task, &m4, NULL); |
||
443 | if (p4 == -1) { |
||
444 | grx_close(); |
||
445 | perror("FFTPlay: Could not create task <fft>\n"); |
||
446 | ll_abort(54); |
||
447 | sys_end(); |
||
448 | } |
||
449 | |||
450 | soft_task_default_model(m5); |
||
451 | soft_task_def_level(m5,1); |
||
452 | soft_task_def_period(m5, PERIOD_EQU); |
||
453 | soft_task_def_met(m5, WCET_EQU); |
||
454 | soft_task_def_group(m5, 1); |
||
455 | soft_task_def_stack(m5,32*1024); |
||
456 | soft_task_def_usemath(m5); |
||
457 | soft_task_def_ctrl_jet(m5); |
||
458 | p5 = task_create("equ", equ_task, &m5, NULL); |
||
459 | if (p5 == -1) { |
||
460 | grx_close(); |
||
461 | perror("FFTPlay: Could not create task <equ>\n"); |
||
462 | ll_abort(54); |
||
463 | perror("FFTPlay: Could not create task <equ>\n"); |
||
464 | sys_end(); |
||
465 | } |
||
466 | |||
467 | soft_task_default_model(m6); |
||
468 | soft_task_def_level(m6,1); |
||
469 | soft_task_def_period(m6, PERIOD_EQU2D); |
||
470 | soft_task_def_met(m6, WCET_EQU2D); |
||
471 | soft_task_def_group(m6, 1); |
||
472 | soft_task_def_stack(m6,32*1024); |
||
473 | soft_task_def_usemath(m6); |
||
474 | soft_task_def_ctrl_jet(m6); |
||
475 | p6 = task_create("equ2D", equ2d_task, &m6, NULL); |
||
476 | if (p6 == -1) { |
||
477 | grx_close(); |
||
478 | perror("FFTPlay: Could not create task <equ2d>\n"); |
||
479 | ll_abort(54); |
||
480 | perror("FFTPlay: Could not create task <equ2D>\n"); |
||
481 | sys_end(); |
||
482 | } |
||
483 | } |
||
484 | |||
485 | void scenario_fftplay(int f) |
||
486 | { |
||
487 | int i,y; |
||
488 | char s[50]; |
||
489 | |||
490 | grx_line(0,WAVE_Y-WAVE_HEIGHT-1,383,WAVE_Y-WAVE_HEIGHT-1,red); |
||
491 | grx_line(0,WAVE_Y+WAVE_HEIGHT+1,383,WAVE_Y+WAVE_HEIGHT+1,red); |
||
492 | grx_line(0,EQU_Y-11 ,383,EQU_Y-11 ,red); |
||
493 | |||
494 | |||
495 | |||
496 | /* lines near the frequencies */ |
||
497 | grx_line(EQU_X +1,EQU_Y,EQU_X +1,EQU_Y+EQU_NSAMPLES-1,red); |
||
498 | grx_line(EQU2D_X-1,EQU_Y,EQU2D_X-1,EQU_Y+EQU_NSAMPLES-1,red); |
||
499 | |||
500 | for (i=0; i<SCENARIO_NLABEL; i++) |
||
501 | { |
||
502 | y = (i*EQU_NSAMPLES)/(SCENARIO_NLABEL-1); |
||
503 | if (i == SCENARIO_NLABEL-1) y--; |
||
504 | grx_line(EQU_X +1,EQU_Y+y,EQU_X +4,EQU_Y+y,red); |
||
505 | grx_line(EQU2D_X-1,EQU_Y+y,EQU2D_X-4,EQU_Y+y,red); |
||
506 | |||
507 | itoa((i*f)/(SCENARIO_NLABEL-1),s); |
||
508 | grx_text(s,EQU_X+20,EQU_Y+y-8,white,black); |
||
509 | } |
||
510 | |||
511 | grx_text("Power Spectrum" , 0, EQU_Y-21, rgb16(0,0,255), black); |
||
512 | grx_text("Power Spectrum Story", EQU2D_X+EQU2D_WIDTH-160, EQU_Y-21, rgb16(0,0,255), black); |
||
513 | grx_text("Waveform" , 0, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black); |
||
514 | } |
||
515 | |||
516 | void compute_params(int *freq,WORD *nsamp) |
||
517 | { |
||
518 | if (*freq< 2000) |
||
519 | { |
||
520 | cprintf("WARNING: frequency less than 2000Hz\n ---> frequency set to 2000Hz\n"); |
||
521 | *freq = 2000; |
||
522 | } |
||
523 | if (*freq<= 8000) { *nsamp = 64; return; } //128 |
||
524 | if (*freq<=16000) { *nsamp = 64; return; } //256 |
||
525 | if (*freq<=24000) { *nsamp = 64; return; } //512 |
||
526 | if (*freq>48000) |
||
527 | { |
||
528 | cprintf("WARNING: frequency greather than 48000Hz\n ---> frequency set to 48000Hz\n"); |
||
529 | *freq = 48000; |
||
530 | } |
||
531 | if (*freq<=48000) { *nsamp = 64; return; } //1024 |
||
532 | } |
||
533 |