Subversion Repositories shark

Rev

Rev 1085 | 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>
96
#include <ports/rfftw.h>
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