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 <ports/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;
}