Subversion Repositories shark

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/*              U N I V E R S I T A      D I      P A V I A              */

/*               DIPARTIMENTO DI INFORMATICA e SISTEMSTICA               */

/*                  corso di INFORMATICA INDUSTRIALE                     */

/*                          prof. G. Buttazzo                            */

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/*                           S I M L I G H T                             */

/*                       progetto con S.H.A.R.K. :                       */

/*                SIMULAZIONE FARI DA PALCO / DISCOTECA                  */

/*                        (C) 2001 by G. Vadruccio                       */

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/*

 * Copyright (C) 2001 G. Vadruccio

 *

 * This program is free software; you can redistribute it and/or modify

 * it under the terms of the GNU General Public License as published by

 * the Free Software Foundation; either version 2 of the License, or

 * (at your option) any later version.

 *

 * This program is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 * GNU General Public License for more details.

 *

 * You should have received a copy of the GNU General Public License

 * along with this program; if not, write to the Free Software

 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

 *

 */

#include <ll/ll.h>

#include <kernel/types.h>

#include <kernel/model.h>

#include <kernel/func.h>

#include <modules/cabs.h>

#include <string.h>

#include <stdlib.h>

#include <semaphore.h>

#include <drivers/keyb.h>

#include <drivers/crtwin.h>

#include <drivers/glib.h>

#include <drivers/sound.h>

#include <rfftw.h>

#include <math.h>

#include "blaster.h"

#define FFT_SCALE (16384.0)                 // fatt. scala usato dal task fft

typedef short SAMPLE;                       // campioni letti da SB a 16-bit

typedef struct {                            // per il CAB cab_camdata

   int start;

   SAMPLE sample[1000];

} campione;

typedef struct {                            // per il CAB cab_prwdata

   fftw_real p[501];

} power;

typedef struct {                            // per il CAB cab_lightdata

   int l[8];

} lights;

CAB cab_camdata;                            // CAB dati campionati dalla SB

CAB cab_pwrdata;                            // CAB dati spettro di potenza

CAB cab_lghdata;                            // CAB dati intensit… luci

sem_t mutex;                                // semaforo di mutua esclusione

campione cam;                               // struttura dei campioni

rfftw_plan plan;                            // usato dalla libreria fft

char fbuf[1000];

int flen,

    WCET_FFT, WCET_LIV, WCET_LIGHT,

    PERIOD_FFT, PERIOD_LIV, PERIOD_LIGHT,

    AGCOP, MICLEV,

    ATT1, ATT2, ATT3, ATT4, ATT5, ATT6, ATT7; // parametri letti da file ext.

void    read_file(void)                     // funzione di lettura file di

{                                           // testo 'PARAM.DAT' contenente

int     err;                                // i parametri di funzionamento

DOS_FILE *fp;

   fp = DOS_fopen("param.dat","r");

   if (!fp) {

      err = DOS_error();

      cprintf("Error %d opening param.dat...\n", err);

      flen = 0;

      return;

   }

   flen = DOS_fread(&fbuf, 1, 325, fp);

   cprintf("Read %d bytes from orbit.dat\n", flen);

   DOS_fclose(fp);

}

void    get_par(void)                       // scansione dei parametri

{

int x = 0;

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &WCET_FFT);         // lettura wcet task fft

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &PERIOD_FFT);       // lettura periodo task fft

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &WCET_LIV);         // lettura wcet task livelli

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &PERIOD_LIV);       // lettura periodo task livelli

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &WCET_LIGHT);       // lettura wcet task light

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &PERIOD_LIGHT);     // lettura periodo task light

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &AGCOP);            // lettura opzione AGC

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &MICLEV);           // letura livello microfonico

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT1);             // lettura attenuazione faro 1

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT2);             // lettura attenuazione faro 2

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT3);             // lettura attenuazione faro 3

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT4);             // lettura attenuazione faro 4

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT5);             // lettura attenuazione faro 5

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT6);             // lettura attenuazione faro 6

   while ((fbuf[x] != ':') && (x < flen)) x++;

   x++;

   sscanf(&fbuf[x], "%d", &ATT7);             // lettura attenuazione faro 7

}

void set_new_palette(void)                  // funzione di preparazione della

{                                           // palette di colori usata

   int k, j;

   for (k=0; k<7; k++)

      for (j=(k*36); j<=(k*36)+4; j++) grx_setcolor(j,0,0,0); // nero

   for (k=252; k<256; k++) grx_setcolor(k,0,0,0);             // nero

   for (k=1; k<=31; k++) {

      grx_setcolor(k+4,(k*2)+1,0,0);               // sfumatura rosso   FARO 1

      grx_setcolor(k+40,(k*2)+1,(k*2)+1,0);        // sfumatura giallo  FARO 2

      grx_setcolor(k+76,0,(k*2)+1,0);              // sfumatura verde   FARO 3

      grx_setcolor(k+112,0,(k*2)+1,(k*2)+1);       // sfumarura azzurra FARO 4

      grx_setcolor(k+148,0,0,(k*2)+1);             // sfumatura blu     FARO 5

      grx_setcolor(k+184,(k*2)+1,0,(k*2)+1);       // sfumatura magenta FARO 6

      grx_setcolor(k+220,(k*2)+1,(k*2)+1,(k*2)+1); // sfumatura bianco  FARO 7

   }

}

void crea_scenario(void)                    // funzione di creazione scenario

{                                           // (trave e campane del palco)

   int j;

   int cs=238;

   for (j=0; j<16; j++) grx_line(40*j,12,(40*j)+20,0,cs);

   for (j=0; j<16; j++) grx_line(40*j+20,0,(40*j)+40,12,cs);

   grx_rect(0,0,639,12,cs);

   for (j=0; j<7; j++) {

      grx_line(49+(j*83),12,49+(j*83),35,cs);

      grx_line(49+(j*83),35,55+(j*83),35,cs);

      grx_box(52+(j*83),32,88+(j*83),59,cs);

      grx_disc(70+(j*83),33,18,cs);

      grx_line(84+(j*83),35,91+(j*83),35,cs);

      grx_line(91+(j*83),35,91+(j*83),12,cs);

   }

}

int raw_infun(void *b)                      // funzione di self-buffering per

{                                           // la lettura dei campioni da SB

    int i;                                  // e scrittura nel cab

    char *w;                                // dei campioni

    SAMPLE *audiobuf = (SAMPLE *)b;

    for (i=0; i<500; i++) {

      cam.sample[cam.start] = audiobuf[i];

      cam.start = (cam.start+1) % 1000;

    }

    w = cab_reserve(cab_camdata);

    memcpy(w, &cam, sizeof(campione));

    cab_putmes(cab_camdata,w);

    return 0;

}

void init_rawdata()                         // funzione per inizializzare

{                                           // il CAB dei campioni letti da SB

  int i;

  char *w;

  cam.start = 0;

  for (i=0; i<1000; i++)

     cam.sample[i] = 0;

  w = cab_reserve(cab_camdata);

  memcpy(w, &cam, sizeof(campione));

  cab_putmes(cab_camdata,w);

}

void fft_close(void *arg)                   // fun. in uscita da task fft_task

{

  rfftw_destroy_plan(plan);

}

TASK fft_task()                             // task per calcolo della fft:

{                                           // legge dal CAB dei dati

  fftw_real in[1000], out[1000];            // campionati e scrive nal CAB

  power power_spectrum;                     // dello spettro di potenza

  campione *p;

  char *m;

  int k, i;

  plan = rfftw_create_plan(1000, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);

  sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT);

  while(1)

  {

    p = (campione *)cab_getmes(cab_camdata);// lettura CAB dati campionati

    for (k = 0, i = p->start;

       k < 1000;

       k++, i = (i+1)%1000)

       in[k] = p->sample[i]/FFT_SCALE;

    cab_unget(cab_camdata,(char *)p);

    rfftw_one(plan, in, out);

    power_spectrum.p[0] = out[0]*out[0];    // calcolo spettro potenza

    for (k = 1; k < 501; ++k)

       power_spectrum.p[k] = out[k]*out[k] + out[1000-k]*out[1000-k];

    power_spectrum.p[500] = out[500]*out[500]; // Nyquist freq.

    m = cab_reserve(cab_pwrdata);           // scrittura nel CAB dello spettro

    memcpy(m, &power_spectrum, sizeof(power));

    cab_putmes(cab_pwrdata,m);

    task_endcycle();

  }

}

TASK livello()                              // task per il calcolo dei livelli

{                                           // luminosi dei fari: legge dal

   power  *p;                               // CAB dello spettro di potenza

   lights *l;                               // e dal CAB dei livelli luminosi

   lights w;                                // e aggiorna il CAB dei livelli

   char *m;                                 // luminosi

   int j[8]={0,0,0,0,0,0,0,0};

   int k[8]={0,0,0,0,0,0,0,0};

   int y[501];

   int i;

   long add;

   while(1) {

      p = (power *)cab_getmes(cab_pwrdata); // lettura del CAB dello spettro

      l = (lights *)cab_getmes(cab_lghdata);// lettura CAB livelli luminosi

      for (i=1; i<=7; i++) j[i]=l->l[i];

      cab_unget(cab_lghdata,(char *)l);

      for (i = 1; i < 501; i++) {

         if ((int)(p->p[i])>650000) y[i]=650000; //controllo range distorsione

         else y[i]=(int)(p->p[i]);

      }

      cab_unget(cab_pwrdata,(char *)p);

      /***************** calocolo livello FARO 1 **************/

      add=0;

      for (i=1; i<4; i++) add+=(long)(y[i]);

      k[1]=(int)(add/ATT1);

      if (k[1]>31) k[1]=31;

      if (k[1]<0) k[1]=0;

      if ((k[1]-j[1])<13) w.l[1]=j[1]-(int)((j[1]/8)+1);

      else w.l[1]=k[1];

      if (w.l[1]>31) w.l[1]=31;

      if (w.l[1]<0) w.l[1]=0;

      /***************** calocolo livello FARO 2 **************/

      add=0;

      for (i=6; i<20; i++) add+=(long)(y[i]/1);

      for (i=90; i<100; i++) add+=(long)(y[i]*3);

      k[2]=(int)(add/ATT2);

      if (k[2]<13) k[2]=0;

      if (k[2]>31) k[2]=31;

      if ((k[2]-j[2])<5) w.l[2]=j[2]-(int)((j[2]/10)+1);

      else w.l[2]=k[2];

      if (w.l[2]>31) w.l[2]=31;

      if (w.l[2]<0) w.l[2]=0;

      /***************** calocolo livello FARO 3 **************/

      add=0;

      for (i=13; i<40; i++) add+=(long)(y[i]);

      k[3]=(int)(add/ATT3);

      if (k[3]<13) k[3]=0;

      if (k[3]>31) k[3]=31;

      if ((k[3]-j[3])<8) w.l[3]=j[3]-3;

      else  w.l[3]=k[3];

      if (w.l[3]>31) w.l[3]=31;

      if (w.l[3]<0) w.l[3]=0;

      /***************** calocolo livello FARO 4 **************/

      add=0;

      for (i=40; i<60; i++) add+=(long)(y[i]);

      k[4]=(int)(add/ATT4);

      if (k[4]<11) k[4]=0;

      if (k[4]>31) k[4]=31;

      if ((k[4]-j[4])<4) w.l[4]=j[4]-3;

      else  w.l[4]=j[4]+3;

      if (w.l[4]>31) w.l[4]=31;

      if (w.l[4]<0) w.l[4]=0;

      /***************** calocolo livello FARO 5 **************/

      add=0;

      for (i=90; i<120; i++) add+=(long)(y[i]);

      for (i=15; i<30; i++) add+=(long)(y[i]/6);

      k[5]=(int)(add/ATT5);

      if (k[5]<13) k[5]=0;

      if (k[5]>31) k[5]=31;

      if ((k[5]-j[5])<4) w.l[5]=j[5]-3;

      else  w.l[5]=k[5];

      if (w.l[5]>31) w.l[5]=31;

      if (w.l[5]<0) w.l[5]=0;

      /***************** calocolo livello FARO 6 **************/

      add=0;

      for (i=170; i<230; i++) add+=(long)(y[i]);

      k[6]=(int)(add/ATT6);

      if (k[6]<13) k[6]=0;

      if (k[6]>31) k[6]=31;

      if ((k[6]-j[6])<6) w.l[6]=j[6]-(int)((j[6]/9)+1);

      else  w.l[6]=j[6]+((k[6]-j[6])/3);

      if (w.l[6]>31) w.l[6]=31;

      if (w.l[6]<0) w.l[6]=0;

      /***************** calocolo livello FARO 7 **************/

      add=0;

      for (i=200; i<450; i++) add+=(long)(y[i]);

      k[7]=(int)(add/ATT7);

      if (k[7]<13) k[7]=0;

      if (k[7]<13) k[7]=0;

      if (k[7]>31) k[7]=31;

      if ((k[7]-j[7])<5) w.l[7]=k[7]-(int)((j[7]/10)+1);

      else  w.l[7]=(int)((k[7]+j[7])/2);

      if (w.l[7]>31) w.l[7]=31;

      if (w.l[7]<0) w.l[7]=0;

      m=cab_reserve(cab_lghdata);           // scrittura CAB livelli luminosi

      memcpy(m, &w, sizeof(lights));

      cab_putmes(cab_lghdata,m);

      task_endcycle();

   }

}

TASK light(void *arg)                       // task per l'accensione grafica

{                                           // delle luci: legge dal CAB dei

   lights *p;                               // livelli luminosi

   int i = (int)arg;                        // i = n. del task

   int a, c, pos, liv;

   while(1) {

      p = (lights *)cab_getmes(cab_lghdata);// lettura CAB livelli luminosi

      liv=p->l[i];                          // livello del faro selezionato

      sem_wait(&mutex);                     // inizio sezione critica

      c=((36*i)-32)+liv;                    // colore del faro selezionato

      pos=(83*i)-13;                        // posizione del faro seleionato

      grx_box(pos-14,60,pos+14,440,c);      // disegna il raggio lumonoso

      for (a=15; a<=18; a++) grx_line(pos-15,60,pos-a,440,c);

      grx_line(pos-15,60,pos-19,440,c-1);

      for (a=20; a<=22; a++) grx_line(pos-16,60,pos-a,440,c-1);

      grx_line(pos-16,60,pos-23,440,c-2);

      for (a=24; a<=26; a++) grx_line(pos-17,60,pos-a,440,c-2);

      for (a=27; a<=30; a++) grx_line(pos-17,60,pos-a,440,c-3);

      for (a=31; a<=34; a++) grx_line(pos-17,60,pos-a,440,c-3);

      for (a=15; a<=18; a++) grx_line(pos+15,60,pos+a,440,c);

      grx_line(pos+15,60,pos+19,440,c-1);

      for (a=20; a<=22; a++) grx_line(pos+16,60,pos+a,440,c-1);

      grx_line(pos+16,60,pos+23,440,c-2);

      for (a=24; a<=26; a++) grx_line(pos+17,60,pos+a,440,c-2);

      for (a=27; a<=30; a++) grx_line(pos+17,60,pos+a,440,c-3);

      for (a=31; a<=34; a++) grx_line(pos+17,60,pos+a,440,c-3);

      sem_post(&mutex);                     // fine sezione critica

      cab_unget(cab_lghdata,(char *)p);

      task_endcycle();

   }

}

                                            // funzione in uscita

void my_close(void *arg)

{

        grx_close();

        kern_printf("Bye Bye!\n");

}

int main(int argc, char **argv)

{

    int modenum;

    int f=48000;                            // frequenza di campionamento

    int i=1;

    HARD_TASK_MODEL m1, m2, m3;

    PID p1, p2, p3;

    cab_camdata = cab_create("camdata", sizeof(campione), 4);

    cab_pwrdata = cab_create("pwr", sizeof(power), 4);

    cab_lghdata = cab_create("lghdata", sizeof (lights),4);

    read_file();

    get_par();

    sound_init((1000 * sizeof(SAMPLE)), NULL); // init sound card

    sound_info();                           // visualizza info sound card

    init_rawdata();                         // init dati usati da raw_infun

    sbmixer_setoutput(0x01,ENABLE);         // abilita output sonoro

    sbmixer_setmiclev(MICLEV);              // imposta sensibilt… input-mic

    sbmixer_setAGC(AGCOP);                  // opzione guadagno automatico

    sound_setfun(raw_infun, (int (*)(void *))-1); // inizia self-buffering

    sound_sample(NULL, f, 0, DMA_OP | PCM16 | MYFUN, NULL);

    cprintf("Press Enter...");

    while (keyb_getchar() != 13);           // premi un tasto per iniziare

    sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); // fun in uscita

    grx_init();                             // attiva grafica

    modenum = grx_getmode(640, 480, 8);

    grx_setmode(modenum);

    set_new_palette();                      // prepara la palette

    crea_scenario();                        // crea lo scenario

    sem_init(&mutex, 0, 1);                 // init graphics mutex

    hard_task_default_model(m1);            // define task m1 (fft)

    hard_task_def_periodic(m1);

    hard_task_def_mit(m1, PERIOD_FFT);

    hard_task_def_wcet(m1, WCET_FFT);

    hard_task_def_usemath(m1);

    hard_task_def_group(m1, 1);

    hard_task_def_stack(m1,32*1024);

    p1 = task_create("fft", fft_task, &m1, NULL); // crea task m1 (fft)

    if (p1 == -1) {

        perror("Could not create task <fft>\n");

        sys_end();

    }

    task_activate(p1);                      // attiva task m1 (fft)

    hard_task_default_model(m2);            // define task m2 (livello)

    hard_task_def_periodic (m2);

    hard_task_def_mit      (m2, PERIOD_LIV);

    hard_task_def_wcet     (m2, WCET_LIV);

    hard_task_def_usemath  (m2);

    hard_task_def_group    (m2, 1);

    hard_task_def_ctrl_jet (m2);

    p2 = task_create("livello", livello, &m2, NULL); // crea task m2 (livello)

    if (p2 == NIL) {

       grx_close();

       perror("Could not create task <livello>");

       sys_abort(1);

    }

    task_activate(p2);                 // attiva task m2 (livello)

    do {

       hard_task_default_model(m3);    // define 7 task m3 (light)

       hard_task_def_periodic (m3);

       hard_task_def_mit      (m3, PERIOD_LIGHT);

       hard_task_def_wcet     (m3, WCET_LIGHT);

       hard_task_def_usemath  (m3);

       hard_task_def_group    (m3, 1);

       hard_task_def_ctrl_jet (m3);

       hard_task_def_arg      (m3, (void *)i);

       p3 = task_create("light", light, &m3, NULL); // crea 7 task m3 (light)

       if (p3 == NIL) {

          grx_close();

          perror("Could not create task <light>");

          sys_abort(1);

       }

       task_activate(p3);                   // attiva 7 task m3 (light)

       i++;

    } while (i<8);

    while (keyb_getchar() != 13);           // premere Invio per terminare

    sys_end();

    return 0;

}