Subversion Repositories shark

Rev

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
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2
/*              U N I V E R S I T A      D I      P A V I A              */
3
/*               DIPARTIMENTO DI INFORMATICA e SISTEMSTICA               */
4
/*                  corso di INFORMATICA INDUSTRIALE                     */
5
/*                          prof. G. Buttazzo                            */
6
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
7
/*                           S I M L I G H T                             */
8
/*                       progetto con S.H.A.R.K. :                       */
9
/*                SIMULAZIONE FARI DA PALCO / DISCOTECA                  */
10
/*                        (C) 2001 by G. Vadruccio                       */
11
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
12
 
13
/*
14
 * Copyright (C) 2001 G. Vadruccio
15
 *
16
 * This program is free software; you can redistribute it and/or modify
17
 * it under the terms of the GNU General Public License as published by
18
 * the Free Software Foundation; either version 2 of the License, or
19
 * (at your option) any later version.
20
 *
21
 * This program is distributed in the hope that it will be useful,
22
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
23
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
24
 * GNU General Public License for more details.
25
 *
26
 * You should have received a copy of the GNU General Public License
27
 * along with this program; if not, write to the Free Software
28
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
29
 *
30
 */
31
 
32
#include <ll/ll.h>
33
#include <kernel/types.h>
34
#include <kernel/model.h>
35
#include <kernel/func.h>
36
#include <modules/cabs.h>
37
#include <string.h>
38
#include <stdlib.h>
39
#include <semaphore.h>
40
#include <drivers/keyb.h>
41
#include <drivers/crtwin.h>
42
#include <drivers/glib.h>
43
#include <drivers/sound.h>
1144 pj 44
#include <rfftw.h>
1085 pj 45
#include <math.h>
46
#include "blaster.h"
47
 
48
#define FFT_SCALE (16384.0)                 // fatt. scala usato dal task fft
49
 
50
typedef short SAMPLE;                       // campioni letti da SB a 16-bit
51
 
52
typedef struct {                            // per il CAB cab_camdata
53
   int start;
54
   SAMPLE sample[1000];
55
} campione;
56
 
57
typedef struct {                            // per il CAB cab_prwdata
58
   fftw_real p[501];
59
} power;
60
 
61
typedef struct {                            // per il CAB cab_lightdata
62
   int l[8];
63
} lights;
64
 
65
CAB cab_camdata;                            // CAB dati campionati dalla SB
66
CAB cab_pwrdata;                            // CAB dati spettro di potenza
67
CAB cab_lghdata;                            // CAB dati intensit… luci
68
sem_t mutex;                                // semaforo di mutua esclusione
69
campione cam;                               // struttura dei campioni
70
rfftw_plan plan;                            // usato dalla libreria fft
71
char fbuf[1000];
72
int flen,
73
    WCET_FFT, WCET_LIV, WCET_LIGHT,
74
    PERIOD_FFT, PERIOD_LIV, PERIOD_LIGHT,
75
    AGCOP, MICLEV,
76
    ATT1, ATT2, ATT3, ATT4, ATT5, ATT6, ATT7; // parametri letti da file ext.
77
 
78
 
79
void    read_file(void)                     // funzione di lettura file di
80
{                                           // testo 'PARAM.DAT' contenente
81
int     err;                                // i parametri di funzionamento
82
DOS_FILE *fp;
83
   fp = DOS_fopen("param.dat","r");
84
   if (!fp) {
85
      err = DOS_error();
86
      cprintf("Error %d opening param.dat...\n", err);
87
      flen = 0;
88
      return;
89
   }
90
   flen = DOS_fread(&fbuf, 1, 325, fp);
91
   cprintf("Read %d bytes from orbit.dat\n", flen);
92
   DOS_fclose(fp);
93
}
94
 
95
 
96
void    get_par(void)                       // scansione dei parametri
97
{
98
int x = 0;
99
 
100
   while ((fbuf[x] != ':') && (x < flen)) x++;
101
   x++;
102
   sscanf(&fbuf[x], "%d", &WCET_FFT);         // lettura wcet task fft
103
   while ((fbuf[x] != ':') && (x < flen)) x++;
104
   x++;
105
   sscanf(&fbuf[x], "%d", &PERIOD_FFT);       // lettura periodo task fft
106
   while ((fbuf[x] != ':') && (x < flen)) x++;
107
   x++;
108
   sscanf(&fbuf[x], "%d", &WCET_LIV);         // lettura wcet task livelli
109
   while ((fbuf[x] != ':') && (x < flen)) x++;
110
   x++;
111
   sscanf(&fbuf[x], "%d", &PERIOD_LIV);       // lettura periodo task livelli
112
   while ((fbuf[x] != ':') && (x < flen)) x++;
113
   x++;
114
   sscanf(&fbuf[x], "%d", &WCET_LIGHT);       // lettura wcet task light
115
   while ((fbuf[x] != ':') && (x < flen)) x++;
116
   x++;
117
   sscanf(&fbuf[x], "%d", &PERIOD_LIGHT);     // lettura periodo task light
118
   while ((fbuf[x] != ':') && (x < flen)) x++;
119
   x++;
120
   sscanf(&fbuf[x], "%d", &AGCOP);            // lettura opzione AGC
121
   while ((fbuf[x] != ':') && (x < flen)) x++;
122
   x++;
123
   sscanf(&fbuf[x], "%d", &MICLEV);           // letura livello microfonico
124
   while ((fbuf[x] != ':') && (x < flen)) x++;
125
   x++;
126
   sscanf(&fbuf[x], "%d", &ATT1);             // lettura attenuazione faro 1
127
   while ((fbuf[x] != ':') && (x < flen)) x++;
128
   x++;
129
   sscanf(&fbuf[x], "%d", &ATT2);             // lettura attenuazione faro 2
130
   while ((fbuf[x] != ':') && (x < flen)) x++;
131
   x++;
132
   sscanf(&fbuf[x], "%d", &ATT3);             // lettura attenuazione faro 3
133
   while ((fbuf[x] != ':') && (x < flen)) x++;
134
   x++;
135
   sscanf(&fbuf[x], "%d", &ATT4);             // lettura attenuazione faro 4
136
   while ((fbuf[x] != ':') && (x < flen)) x++;
137
   x++;
138
   sscanf(&fbuf[x], "%d", &ATT5);             // lettura attenuazione faro 5
139
   while ((fbuf[x] != ':') && (x < flen)) x++;
140
   x++;
141
   sscanf(&fbuf[x], "%d", &ATT6);             // lettura attenuazione faro 6
142
   while ((fbuf[x] != ':') && (x < flen)) x++;
143
   x++;
144
   sscanf(&fbuf[x], "%d", &ATT7);             // lettura attenuazione faro 7
145
}
146
 
147
 
148
void set_new_palette(void)                  // funzione di preparazione della
149
{                                           // palette di colori usata
150
   int k, j;
151
 
152
   for (k=0; k<7; k++)
153
      for (j=(k*36); j<=(k*36)+4; j++) grx_setcolor(j,0,0,0); // nero
154
   for (k=252; k<256; k++) grx_setcolor(k,0,0,0);             // nero
155
   for (k=1; k<=31; k++) {
156
      grx_setcolor(k+4,(k*2)+1,0,0);               // sfumatura rosso   FARO 1
157
      grx_setcolor(k+40,(k*2)+1,(k*2)+1,0);        // sfumatura giallo  FARO 2
158
      grx_setcolor(k+76,0,(k*2)+1,0);              // sfumatura verde   FARO 3
159
      grx_setcolor(k+112,0,(k*2)+1,(k*2)+1);       // sfumarura azzurra FARO 4
160
      grx_setcolor(k+148,0,0,(k*2)+1);             // sfumatura blu     FARO 5
161
      grx_setcolor(k+184,(k*2)+1,0,(k*2)+1);       // sfumatura magenta FARO 6
162
      grx_setcolor(k+220,(k*2)+1,(k*2)+1,(k*2)+1); // sfumatura bianco  FARO 7
163
   }
164
}
165
 
166
 
167
void crea_scenario(void)                    // funzione di creazione scenario
168
{                                           // (trave e campane del palco)
169
   int j;
170
   int cs=238;
171
 
172
   for (j=0; j<16; j++) grx_line(40*j,12,(40*j)+20,0,cs);
173
   for (j=0; j<16; j++) grx_line(40*j+20,0,(40*j)+40,12,cs);
174
   grx_rect(0,0,639,12,cs);
175
   for (j=0; j<7; j++) {
176
      grx_line(49+(j*83),12,49+(j*83),35,cs);
177
      grx_line(49+(j*83),35,55+(j*83),35,cs);
178
      grx_box(52+(j*83),32,88+(j*83),59,cs);
179
      grx_disc(70+(j*83),33,18,cs);
180
      grx_line(84+(j*83),35,91+(j*83),35,cs);
181
      grx_line(91+(j*83),35,91+(j*83),12,cs);
182
   }
183
}
184
 
185
 
186
int raw_infun(void *b)                      // funzione di self-buffering per
187
{                                           // la lettura dei campioni da SB
188
    int i;                                  // e scrittura nel cab
189
    char *w;                                // dei campioni
190
    SAMPLE *audiobuf = (SAMPLE *)b;
191
 
192
    for (i=0; i<500; i++) {
193
      cam.sample[cam.start] = audiobuf[i];
194
      cam.start = (cam.start+1) % 1000;
195
    }
196
    w = cab_reserve(cab_camdata);
197
    memcpy(w, &cam, sizeof(campione));
198
    cab_putmes(cab_camdata,w);
199
    return 0;
200
}
201
 
202
 
203
void init_rawdata()                         // funzione per inizializzare
204
{                                           // il CAB dei campioni letti da SB
205
  int i;
206
  char *w;
207
 
208
  cam.start = 0;
209
  for (i=0; i<1000; i++)
210
     cam.sample[i] = 0;
211
  w = cab_reserve(cab_camdata);
212
  memcpy(w, &cam, sizeof(campione));
213
  cab_putmes(cab_camdata,w);
214
}
215
 
216
 
217
void fft_close(void *arg)                   // fun. in uscita da task fft_task
218
{
219
  rfftw_destroy_plan(plan);
220
}
221
 
222
 
223
TASK fft_task()                             // task per calcolo della fft:
224
{                                           // legge dal CAB dei dati
225
  fftw_real in[1000], out[1000];            // campionati e scrive nal CAB
226
  power power_spectrum;                     // dello spettro di potenza
227
  campione *p;
228
  char *m;
229
  int k, i;
230
 
231
  plan = rfftw_create_plan(1000, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);
232
  sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT);
233
  while(1)
234
  {
235
    p = (campione *)cab_getmes(cab_camdata);// lettura CAB dati campionati
236
    for (k = 0, i = p->start;
237
       k < 1000;
238
       k++, i = (i+1)%1000)
239
       in[k] = p->sample[i]/FFT_SCALE;
240
    cab_unget(cab_camdata,(char *)p);
241
    rfftw_one(plan, in, out);
242
    power_spectrum.p[0] = out[0]*out[0];    // calcolo spettro potenza
243
    for (k = 1; k < 501; ++k)
244
       power_spectrum.p[k] = out[k]*out[k] + out[1000-k]*out[1000-k];
245
    power_spectrum.p[500] = out[500]*out[500]; // Nyquist freq.
246
    m = cab_reserve(cab_pwrdata);           // scrittura nel CAB dello spettro
247
    memcpy(m, &power_spectrum, sizeof(power));
248
    cab_putmes(cab_pwrdata,m);
249
    task_endcycle();
250
  }
251
}
252
 
253
 
254
TASK livello()                              // task per il calcolo dei livelli
255
{                                           // luminosi dei fari: legge dal
256
   power  *p;                               // CAB dello spettro di potenza
257
   lights *l;                               // e dal CAB dei livelli luminosi
258
   lights w;                                // e aggiorna il CAB dei livelli
259
   char *m;                                 // luminosi
260
   int j[8]={0,0,0,0,0,0,0,0};
261
   int k[8]={0,0,0,0,0,0,0,0};
262
   int y[501];
263
   int i;
264
   long add;
265
   while(1) {
266
      p = (power *)cab_getmes(cab_pwrdata); // lettura del CAB dello spettro
267
      l = (lights *)cab_getmes(cab_lghdata);// lettura CAB livelli luminosi
268
      for (i=1; i<=7; i++) j[i]=l->l[i];
269
      cab_unget(cab_lghdata,(char *)l);
270
      for (i = 1; i < 501; i++) {
271
         if ((int)(p->p[i])>650000) y[i]=650000; //controllo range distorsione
272
         else y[i]=(int)(p->p[i]);
273
      }
274
      cab_unget(cab_pwrdata,(char *)p);
275
      /***************** calocolo livello FARO 1 **************/
276
      add=0;
277
      for (i=1; i<4; i++) add+=(long)(y[i]);
278
      k[1]=(int)(add/ATT1);
279
      if (k[1]>31) k[1]=31;
280
      if (k[1]<0) k[1]=0;
281
      if ((k[1]-j[1])<13) w.l[1]=j[1]-(int)((j[1]/8)+1);
282
      else w.l[1]=k[1];
283
      if (w.l[1]>31) w.l[1]=31;
284
      if (w.l[1]<0) w.l[1]=0;
285
      /***************** calocolo livello FARO 2 **************/
286
      add=0;
287
      for (i=6; i<20; i++) add+=(long)(y[i]/1);
288
      for (i=90; i<100; i++) add+=(long)(y[i]*3);
289
      k[2]=(int)(add/ATT2);
290
      if (k[2]<13) k[2]=0;
291
      if (k[2]>31) k[2]=31;
292
      if ((k[2]-j[2])<5) w.l[2]=j[2]-(int)((j[2]/10)+1);
293
      else w.l[2]=k[2];
294
      if (w.l[2]>31) w.l[2]=31;
295
      if (w.l[2]<0) w.l[2]=0;
296
      /***************** calocolo livello FARO 3 **************/
297
      add=0;
298
      for (i=13; i<40; i++) add+=(long)(y[i]);
299
      k[3]=(int)(add/ATT3);
300
      if (k[3]<13) k[3]=0;
301
      if (k[3]>31) k[3]=31;
302
      if ((k[3]-j[3])<8) w.l[3]=j[3]-3;
303
      else  w.l[3]=k[3];
304
      if (w.l[3]>31) w.l[3]=31;
305
      if (w.l[3]<0) w.l[3]=0;
306
      /***************** calocolo livello FARO 4 **************/
307
      add=0;
308
      for (i=40; i<60; i++) add+=(long)(y[i]);
309
      k[4]=(int)(add/ATT4);
310
      if (k[4]<11) k[4]=0;
311
      if (k[4]>31) k[4]=31;
312
      if ((k[4]-j[4])<4) w.l[4]=j[4]-3;
313
      else  w.l[4]=j[4]+3;
314
      if (w.l[4]>31) w.l[4]=31;
315
      if (w.l[4]<0) w.l[4]=0;
316
      /***************** calocolo livello FARO 5 **************/
317
      add=0;
318
      for (i=90; i<120; i++) add+=(long)(y[i]);
319
      for (i=15; i<30; i++) add+=(long)(y[i]/6);
320
      k[5]=(int)(add/ATT5);
321
      if (k[5]<13) k[5]=0;
322
      if (k[5]>31) k[5]=31;
323
      if ((k[5]-j[5])<4) w.l[5]=j[5]-3;
324
      else  w.l[5]=k[5];
325
      if (w.l[5]>31) w.l[5]=31;
326
      if (w.l[5]<0) w.l[5]=0;
327
      /***************** calocolo livello FARO 6 **************/
328
      add=0;
329
      for (i=170; i<230; i++) add+=(long)(y[i]);
330
      k[6]=(int)(add/ATT6);
331
      if (k[6]<13) k[6]=0;
332
      if (k[6]>31) k[6]=31;
333
      if ((k[6]-j[6])<6) w.l[6]=j[6]-(int)((j[6]/9)+1);
334
      else  w.l[6]=j[6]+((k[6]-j[6])/3);
335
      if (w.l[6]>31) w.l[6]=31;
336
      if (w.l[6]<0) w.l[6]=0;
337
      /***************** calocolo livello FARO 7 **************/
338
      add=0;
339
      for (i=200; i<450; i++) add+=(long)(y[i]);
340
      k[7]=(int)(add/ATT7);
341
      if (k[7]<13) k[7]=0;
342
      if (k[7]<13) k[7]=0;
343
      if (k[7]>31) k[7]=31;
344
      if ((k[7]-j[7])<5) w.l[7]=k[7]-(int)((j[7]/10)+1);
345
      else  w.l[7]=(int)((k[7]+j[7])/2);
346
      if (w.l[7]>31) w.l[7]=31;
347
      if (w.l[7]<0) w.l[7]=0;
348
 
349
      m=cab_reserve(cab_lghdata);           // scrittura CAB livelli luminosi
350
      memcpy(m, &w, sizeof(lights));
351
      cab_putmes(cab_lghdata,m);
352
      task_endcycle();
353
   }
354
}
355
 
356
 
357
TASK light(void *arg)                       // task per l'accensione grafica
358
{                                           // delle luci: legge dal CAB dei
359
   lights *p;                               // livelli luminosi
360
   int i = (int)arg;                        // i = n. del task
361
   int a, c, pos, liv;
362
 
363
   while(1) {
364
      p = (lights *)cab_getmes(cab_lghdata);// lettura CAB livelli luminosi
365
      liv=p->l[i];                          // livello del faro selezionato
366
      sem_wait(&mutex);                     // inizio sezione critica
367
      c=((36*i)-32)+liv;                    // colore del faro selezionato
368
      pos=(83*i)-13;                        // posizione del faro seleionato
369
      grx_box(pos-14,60,pos+14,440,c);      // disegna il raggio lumonoso
370
      for (a=15; a<=18; a++) grx_line(pos-15,60,pos-a,440,c);
371
      grx_line(pos-15,60,pos-19,440,c-1);
372
      for (a=20; a<=22; a++) grx_line(pos-16,60,pos-a,440,c-1);
373
      grx_line(pos-16,60,pos-23,440,c-2);
374
      for (a=24; a<=26; a++) grx_line(pos-17,60,pos-a,440,c-2);
375
      for (a=27; a<=30; a++) grx_line(pos-17,60,pos-a,440,c-3);
376
      for (a=31; a<=34; a++) grx_line(pos-17,60,pos-a,440,c-3);
377
      for (a=15; a<=18; a++) grx_line(pos+15,60,pos+a,440,c);
378
      grx_line(pos+15,60,pos+19,440,c-1);
379
      for (a=20; a<=22; a++) grx_line(pos+16,60,pos+a,440,c-1);
380
      grx_line(pos+16,60,pos+23,440,c-2);
381
      for (a=24; a<=26; a++) grx_line(pos+17,60,pos+a,440,c-2);
382
      for (a=27; a<=30; a++) grx_line(pos+17,60,pos+a,440,c-3);
383
      for (a=31; a<=34; a++) grx_line(pos+17,60,pos+a,440,c-3);
384
      sem_post(&mutex);                     // fine sezione critica
385
      cab_unget(cab_lghdata,(char *)p);
386
      task_endcycle();
387
   }
388
}
389
 
390
                                            // funzione in uscita
391
void my_close(void *arg)
392
{
393
        grx_close();
394
        kern_printf("Bye Bye!\n");
395
}
396
 
397
 
398
int main(int argc, char **argv)
399
{
400
    int modenum;
401
    int f=48000;                            // frequenza di campionamento
402
    int i=1;
403
    HARD_TASK_MODEL m1, m2, m3;
404
    PID p1, p2, p3;
405
 
406
    cab_camdata = cab_create("camdata", sizeof(campione), 4);
407
    cab_pwrdata = cab_create("pwr", sizeof(power), 4);
408
    cab_lghdata = cab_create("lghdata", sizeof (lights),4);
409
    read_file();
410
    get_par();
411
    sound_init((1000 * sizeof(SAMPLE)), NULL); // init sound card
412
    sound_info();                           // visualizza info sound card
413
    init_rawdata();                         // init dati usati da raw_infun
414
    sbmixer_setoutput(0x01,ENABLE);         // abilita output sonoro
415
    sbmixer_setmiclev(MICLEV);              // imposta sensibilt… input-mic
416
    sbmixer_setAGC(AGCOP);                  // opzione guadagno automatico
417
    sound_setfun(raw_infun, (int (*)(void *))-1); // inizia self-buffering
418
    sound_sample(NULL, f, 0, DMA_OP | PCM16 | MYFUN, NULL);
419
    cprintf("Press Enter...");
420
    while (keyb_getchar() != 13);           // premi un tasto per iniziare
421
    sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); // fun in uscita
422
    grx_init();                             // attiva grafica
423
    modenum = grx_getmode(640, 480, 8);
424
    grx_setmode(modenum);
425
    set_new_palette();                      // prepara la palette
426
    crea_scenario();                        // crea lo scenario
427
    sem_init(&mutex, 0, 1);                 // init graphics mutex
428
 
429
    hard_task_default_model(m1);            // define task m1 (fft)
430
    hard_task_def_periodic(m1);
431
    hard_task_def_mit(m1, PERIOD_FFT);
432
    hard_task_def_wcet(m1, WCET_FFT);
433
    hard_task_def_usemath(m1);
434
    hard_task_def_group(m1, 1);
435
    hard_task_def_stack(m1,32*1024);
436
    p1 = task_create("fft", fft_task, &m1, NULL); // crea task m1 (fft)
437
    if (p1 == -1) {
438
        perror("Could not create task <fft>\n");
439
        sys_end();
440
    }
441
    task_activate(p1);                      // attiva task m1 (fft)
442
 
443
    hard_task_default_model(m2);            // define task m2 (livello)
444
    hard_task_def_periodic (m2);
445
    hard_task_def_mit      (m2, PERIOD_LIV);
446
    hard_task_def_wcet     (m2, WCET_LIV);
447
    hard_task_def_usemath  (m2);
448
    hard_task_def_group    (m2, 1);
449
    hard_task_def_ctrl_jet (m2);
450
    p2 = task_create("livello", livello, &m2, NULL); // crea task m2 (livello)
451
    if (p2 == NIL) {
452
       grx_close();
453
       perror("Could not create task <livello>");
454
       sys_abort(1);
455
    }
456
    task_activate(p2);                 // attiva task m2 (livello)
457
 
458
    do {
459
       hard_task_default_model(m3);    // define 7 task m3 (light)
460
       hard_task_def_periodic (m3);
461
       hard_task_def_mit      (m3, PERIOD_LIGHT);
462
       hard_task_def_wcet     (m3, WCET_LIGHT);
463
       hard_task_def_usemath  (m3);
464
       hard_task_def_group    (m3, 1);
465
       hard_task_def_ctrl_jet (m3);
466
       hard_task_def_arg      (m3, (void *)i);
467
       p3 = task_create("light", light, &m3, NULL); // crea 7 task m3 (light)
468
       if (p3 == NIL) {
469
          grx_close();
470
          perror("Could not create task <light>");
471
          sys_abort(1);
472
       }
473
       task_activate(p3);                   // attiva 7 task m3 (light)
474
       i++;
475
    } while (i<8);
476
 
477
    while (keyb_getchar() != 13);           // premere Invio per terminare
478
    sys_end();
479
    return 0;
480
}
481
 
482