home *** CD-ROM | disk | FTP | other *** search
/ M.u.C.S. Disc 2000 / MUCS2000.iso / falcon / m_analyz / source / calccoef / calccoef.c next >
Encoding:
C/C++ Source or Header  |  1995-02-18  |  14.9 KB  |  475 lines
  1. /*                    Calculate FIR-filter coefficients                    */
  2. /*    main routines from 'firdemo' by Frank Kiesow and    */
  3. /*    shell written by Roel van de Kraats                                */
  4.  
  5. /* tabsize=2    */
  6.  
  7. #include <stdio.h>
  8. #include <stdlib.h> 
  9. #include <tos.h>
  10. #include <math.h>
  11. #include <ext.h>
  12. #include <string.h>
  13. #include <aes.h>
  14. #include "calccoef.h"
  15.  
  16. #define FALSE                 0
  17. #define TRUE                  1
  18. #define    MAX_COEFF         1000
  19. #define STFILELEN     13    /* Maximale Länge eines Filenamens     */
  20. #define STPATHLEN     64    /* Maximale Länge eines Pfades         */
  21. #define EOS           '\0'  /* Ende eines Strings                  */
  22. #define BACKSLASH     '\\'
  23.  
  24. typedef char DSP_WORD[3];
  25.  
  26. long        get_value(OBJECT *tree,int obj);
  27. int         new_filter(void);
  28. int         fpfix(float src,DSP_WORD dest);
  29. void         fir_tp(int n,double ta,double wg);
  30. void       fir_hp(int n,double ta,double wg);
  31. int       fir_bp(int n,double ta,double low_wg,double up_wg);
  32. int       fir_bs(int n,double ta,double low_wg,double up_wg);
  33. void         fir_hamming_win(int ordnung);
  34. void         fir_hanning_win(int ordnung);
  35. void         fir_blackman_win(int ordnung);
  36. void         fir_bartlett_win(int ordnung,double smpl_freq);
  37. void         fir_lanczos_win(int ordnung);
  38. void         fir_fejer_win(int ordnung);
  39. void        mouse_on( void );
  40. void        mouse_off( void );
  41. void        event_loop( void );
  42. void        build_fname( char *dest, char *s1, char *s2 );
  43. int            op_fbox( char *string );
  44. void        save_filter(void);
  45.  
  46. char Path[STPATHLEN];
  47. char Name[STPATHLEN + STFILELEN];
  48.  
  49. DSP_WORD    coeff[MAX_COEFF];     /* Maximal xx Koeffizienten a' 3 Bytes              */
  50. double        a_k[MAX_COEFF];        /* Maximal 100 Filterkoeffizienten                        */
  51. OBJECT        *maintree;
  52.  
  53. double     smpl_freq,
  54.                 lower_freq,
  55.                 upper_freq;
  56. int         n_coeff;
  57.  
  58. int main(void)
  59. {
  60.     int x,y,w,h, exit_obj;
  61.     if ( appl_init( ) >=0 )
  62.     {
  63.         graf_mouse(ARROW,0);
  64.         if (rsrc_load("CALCCOEF.RSC")!=0)
  65.         {
  66.             Path[0]=(char)(Dgetdrv()+65);
  67.             strcat(Path,":");
  68.             Dgetpath(Name,0);
  69.             strcat(Path,Name);
  70.             strcat(Path,"\\*.FTR");
  71.  
  72.             rsrc_gaddr(0,MAINTREE,&maintree);
  73.  
  74.             form_center(maintree,&x,&y,&w,&h);
  75.             form_dial(FMD_START,0,0,0,0,x,y,w,h);
  76.             objc_draw(maintree,ROOT,MAX_DEPTH,x,y,w,h);
  77.             do
  78.             {
  79.                 exit_obj = form_do(maintree,0);
  80.                 switch(exit_obj)
  81.                 {
  82.                     case    LOWPASS      :    maintree[LOWERFREQ].ob_state |= DISABLED;
  83.                                                         maintree[UPPERFREQ].ob_state &= ~DISABLED;
  84.                                                         maintree[LOWERFREQ].ob_flags &= ~EDITABLE;
  85.                                                         maintree[UPPERFREQ].ob_flags |= EDITABLE;
  86.                                                         objc_draw(maintree,LOWERFREQ,MAX_DEPTH,x,y,w,h);
  87.                                                         objc_draw(maintree,UPPERFREQ,MAX_DEPTH,x,y,w,h);
  88.                                                         break;
  89.                     case    HIGHPASS     :    maintree[LOWERFREQ].ob_state &= ~DISABLED;
  90.                                                         maintree[UPPERFREQ].ob_state |= DISABLED;
  91.                                                         maintree[LOWERFREQ].ob_flags |= EDITABLE;
  92.                                                         maintree[UPPERFREQ].ob_flags &= ~EDITABLE;
  93.                                                         objc_draw(maintree,LOWERFREQ,MAX_DEPTH,x,y,w,h);
  94.                                                         objc_draw(maintree,UPPERFREQ,MAX_DEPTH,x,y,w,h);
  95.                                                         break;
  96.                     case    BANDPASS     :    
  97.                     case     BANDSTOP     :    maintree[LOWERFREQ].ob_state &= ~DISABLED;
  98.                                                         maintree[UPPERFREQ].ob_state &= ~DISABLED;
  99.                                                         maintree[LOWERFREQ].ob_flags |= EDITABLE;
  100.                                                         maintree[UPPERFREQ].ob_flags |= EDITABLE;
  101.                                                         objc_draw(maintree,LOWERFREQ,MAX_DEPTH,x,y,w,h);
  102.                                                         objc_draw(maintree,UPPERFREQ,MAX_DEPTH,x,y,w,h);
  103.                                                         break;
  104.                 }                                            
  105.             }
  106.             while((exit_obj != CALCBUT) && (exit_obj != CANCELBUT));
  107.             if (exit_obj==CALCBUT)
  108.             {
  109.                 new_filter();
  110.                 if (op_fbox("Save Filter Data")==1)
  111.                     save_filter();
  112.             }
  113.             form_dial(FMD_FINISH,0,0,0,0,x,y,w,h);
  114.  
  115.             rsrc_free( );
  116.         }
  117.         else
  118.             form_alert(1,"[3][RSC-File not found][Exit]");
  119.         appl_exit( );
  120.     }
  121.     return(0);
  122. }
  123.  
  124. long get_value(OBJECT *tree,int obj)
  125. {
  126.     char s[10];
  127.     int i;
  128.     strcpy(s,tree[obj].ob_spec.tedinfo->te_ptext);
  129.     for (i=0;i<9;i++)
  130.     {
  131.         if (s[i]=='_')
  132.             s[i]=' ';
  133.     }
  134.     return atol(s);
  135. }
  136. int fpfix(float src,DSP_WORD dest)
  137. {
  138.     float x;
  139.     unsigned long *y1;                    /* Alias für (float) x                                            */
  140.     unsigned long    y0;
  141.     char *ptr;                                    /* Alias für (unsigned long) y0                            */
  142.     y1 = (unsigned long *)&x;
  143.     ptr = (char *)&y0;
  144.     if(fabs(src) >= 2.0e-7)
  145.     { 
  146.         x = src;
  147.         y0 = *y1<<8;                            /* Mantisse (fast)ganz nach links schieben    */
  148.     
  149.         *y1 >>= 23;                                /* Exponent in LSB                                                    */
  150.         *y1 &= 0xffL;                            /* Vorzeichen rausschmeissen                                */
  151.         *y1 = 0x7fL - *y1;                /* Anzahl der Rechtsshifts berechnen                */
  152.     
  153.         y0 |= 0x80000000uL;                /* Führende '1' einfügen                                        */
  154.         y0 >>= *y1;                              /* Denormalisieren...                                                */
  155.         if(src < 0.0)                            /* (Float) x < 0.0 ?                                                */
  156.           y0 = ~y0;                                /* Dann 1'er Komplement bilden                            */
  157.         y0 >>= 7;                                    /* ... 24 Bit Wert erzeugen                                    */
  158.         if(y0 & 0x01L)                        /* Letzte rausgeschobene Stelle  = 1?                */
  159.             y0++;                                        /* ...dann aufrunden                                                */    
  160.         y0 >>= 1;    
  161.     }    
  162.     else
  163.       y0 = 0L;
  164.     dest[0] = ptr[1];
  165.     dest[1] = ptr[2];
  166.     dest[2] = ptr[3];
  167.     return 1; 
  168. }
  169.  
  170.  
  171. void fir_tp(int n, double ta, double wg)  /* FIR Tiefpass berechnen       */
  172. {
  173.   int k;
  174.   double omega;
  175.   double si;
  176.   double si_k;
  177.   double *coeff_ptr = a_k;
  178.   omega = 2.0 * (wg/ta);
  179.   si = M_PI * omega;
  180.   *coeff_ptr++ = omega;       /* omega * Si(0) = omega * 1.0                      */
  181.   for(k=1;k<n;k++)
  182.   {
  183.       si_k = (double)k * si;
  184.     *coeff_ptr++ = omega * sin(si_k)/(si_k);
  185.   } 
  186. }
  187.  
  188. void fir_hp(int n, double ta, double wg) /* FIR Hochpass berechnen        */
  189. {
  190.   int k;
  191.   fir_tp(n,ta,wg);                       /* Zuerst den entsprechenden     */
  192.   a_k[0] = 1.0 - a_k[0];                                  /* Tiefpass berechnen ... dann   */
  193.   for (k=1;k<n;k++)                      /*  von Allpass(= 1)subtrahieren */
  194.     a_k[k] = -a_k[k];
  195. }
  196.  
  197. int fir_bp(int n, double ta, double low_wg,double up_wg)
  198. {                               /* FIR Bandpass berechnen                 */
  199.   int k;                        /* Ergibt sich als Differenz zweier       */
  200.   double *h_vec;                /* Tiefpasse                              */  
  201.   h_vec = (double *)Malloc((n+1) * sizeof(double));/* Hilfsvector erzeugen*/
  202.   if(h_vec == 0L)
  203.   {
  204.       form_alert(1,"[3][Out of Memory][Exit]");
  205.     return FALSE;
  206.   }  
  207.   fir_tp(n,ta,low_wg);          /* Zuerst untere Grenzfrequenz berechnen  */
  208.   for (k=0;k<n;k++)
  209.     h_vec[k] = a_k[k];                    /* ... und sichern                        */
  210.   fir_tp(n,ta,up_wg);           /* Obere Grenzfrequenz berechnen          */
  211.   for (k=0;k<n;k++)
  212.     a_k[k] -= h_vec[k];                        /* Differenz bilden                     */
  213.   Mfree(h_vec);
  214.   return TRUE;
  215. }
  216.  
  217. int fir_bs(int n, double ta, double low_wg,double up_wg)
  218. {                               /* FIR Bandsperre berechnen               */
  219.   int k;                        /* Ergibt sich aus der Differenz eines    */
  220.   if(!fir_bp(n,ta,low_wg,up_wg))/* Allpass(=1) und eines Bandpass         */
  221.     return FALSE;
  222.   a_k[0] =  1.0 - a_k[0];
  223.   for (k=1;k<n;k++)
  224.     a_k[k] = -a_k[k];
  225.   return TRUE;  
  226. }
  227.  
  228.  
  229. void fir_hamming_win(int ordnung)   /* Fensterfunktion                    */
  230. {
  231.   int n;
  232.   for (n=0;n<ordnung;n++)
  233.     a_k[n] *= (0.54+0.46*cos(M_PI*n/ordnung));
  234. }
  235.  
  236.  
  237. void fir_hanning_win(int ordnung)   /* Fensterfunktion                    */
  238. {
  239.   int n;
  240.   for (n=0;n<ordnung;n++)
  241.     a_k[n] *= (0.5+0.5*cos(M_PI*n/ordnung));
  242. }
  243.  
  244.  
  245. void fir_blackman_win(int ordnung)  /* Fensterfunktion                    */
  246. {
  247.   int n;
  248.   for (n=0;n<ordnung;n++)
  249.     a_k[n] *= (0.42+0.5*cos(M_PI*n/ordnung)+
  250.                          0.08*cos(2*M_PI*n/ordnung));
  251. }
  252.  
  253.  
  254. void fir_bartlett_win(int ordnung,double smpl_freq)
  255. {                                   /* Fensterfunktion                    */
  256.   int n;
  257.   double nenner;
  258.   nenner = ordnung * smpl_freq;
  259.   for (n=0;n<ordnung;n++)
  260.     a_k[n] *= (1-fabs(n*smpl_freq)/nenner);
  261. }
  262.  
  263.  
  264. void fir_lanczos_win(int ordnung)   /* Fensterfunktion                    */
  265. {
  266.   int n;
  267.   for (n=1;n<ordnung;n++)
  268.     a_k[n] *= (sin(M_PI*n/ordnung)/(M_PI*n/ordnung));
  269. }
  270.  
  271.  
  272. void fir_fejer_win(int ordnung)     /* Fensterfunktion                    */
  273. {
  274.   int n;
  275.   for (n=0;n<ordnung;n++)
  276.     a_k[n] *= (1-fabs(n/ordnung));
  277. }
  278.  
  279. int new_filter(void)
  280. {
  281.     double sum_coeff; 
  282.     int i;
  283.     n_coeff = (int)get_value(maintree,ORDER);
  284.     lower_freq = get_value(maintree,LOWERFREQ);
  285.     upper_freq = get_value(maintree,UPPERFREQ);
  286.     smpl_freq = get_value(maintree,SAMPLEFREQ);
  287.     sum_coeff = 0.0;
  288.     if(maintree[LOWPASS].ob_state == SELECTED)
  289.         fir_tp(n_coeff,smpl_freq,upper_freq);
  290.     else    
  291.         if(maintree[HIGHPASS].ob_state == SELECTED)
  292.             fir_hp(n_coeff,smpl_freq,lower_freq);
  293.         else
  294.           if(maintree[BANDPASS].ob_state == SELECTED)
  295.           {
  296.                 if(!fir_bp(n_coeff,smpl_freq,lower_freq,upper_freq))
  297.                     return FALSE;
  298.             }    
  299.             else
  300.               if(maintree[BANDSTOP].ob_state == SELECTED)
  301.                     if(!fir_bs(n_coeff,smpl_freq,lower_freq,upper_freq))
  302.                       return FALSE;
  303.     
  304.     if(maintree[BARTLETT].ob_state == SELECTED)
  305.         fir_bartlett_win(n_coeff,smpl_freq);
  306.     else    
  307.         if(maintree[BLACKMAN].ob_state == SELECTED)
  308.             fir_blackman_win(n_coeff);    
  309.         else    
  310.             if(maintree[FEJER].ob_state == SELECTED)
  311.                 fir_fejer_win(n_coeff);    
  312.             else    
  313.                 if(maintree[HAMM].ob_state == SELECTED)
  314.                     fir_hamming_win(n_coeff);
  315.                 else
  316.                     if(maintree[HANN].ob_state == SELECTED)
  317.                         fir_hanning_win(n_coeff);
  318.                     else
  319.                         if(maintree[LANCZOS].ob_state == SELECTED)
  320.                             fir_lanczos_win(n_coeff);
  321.                         else    
  322.                             if(maintree[RECT].ob_state == SELECTED)        /* Default        */
  323.                                 ;
  324.     for(i=0;i<n_coeff;i++)                /* Betrag der Filterkoefizienten = 1            */
  325.       sum_coeff += fabs(a_k[i]);
  326.     sum_coeff = 1.0/sum_coeff;  
  327.     for(i=0;i<n_coeff;i++)                /* Koeffizienten von Double- in                        */
  328.     {                                                            /* 24 Bit Festpunktformat wandeln                    */    
  329.         a_k[i] *= sum_coeff;
  330.         fpfix((float)a_k[i],coeff[i]);
  331.     }
  332. /*
  333.     for(i=0;i<10/*n_coeff*/;i++)
  334.         printf("%i: Floating Point: %f    DSP Word: $%02x%02x%02x\n",i,a_k[i],(int)coeff[i][0] & 0xff,(int)coeff[i][1] & 0xff,(int)coeff[i][2] & 0xff);
  335. */
  336.     return TRUE;
  337. }            
  338. int op_fbox( char *string )
  339. {
  340.    char n[STFILELEN];                  /* Buffer für Dateinamen         */
  341.    int  b;                             /* Enthält Code des Buttons der  */
  342.                                        /* zum Abbruch der Dateiauswahl  */
  343.                                        /* führte.                       */
  344.  
  345.    int version;
  346.  
  347.    *n = EOS;                           /* Dateinamen löschen.           */
  348.  
  349.    version = Sversion ( );             /* Berechne die GEMDOS-Version,  */
  350.    version >>= 8;                      /* da fsel_exinput erst ab  */
  351.                                        /* Version 1.40 zur Verfügung    */
  352.                                        /* steht.                        */
  353.  
  354.    if ( version <= 20 )
  355.                                        /* Dateiauswahl.                 */
  356.       fsel_input( Path, n, &b );
  357.    else
  358.       fsel_exinput( Path, n, &b, string );
  359.  
  360.    build_fname( Name, Path, n );
  361.  
  362.    return ( b );
  363. }
  364.  
  365. /* -------------------------------------------------------------------- */
  366. /*                                                  build a file name   */
  367. /*       void build_fname( char *dest, char *s1, char *s2 );            */
  368. /*                                                                      */
  369. /*       Konkatoniere Pfadnamen und Dateinamen.                         */
  370. /*                                                                      */
  371. /*       -> dest                 Zielstring.                            */
  372. /*          s1                   Pfadname.                              */
  373. /*          s2                   Dateiname.                             */
  374. /*                                                                      */
  375. /*       <-                      Ergebnis befindet sich in 'dest'.      */
  376. /* -------------------------------------------------------------------- */
  377.  
  378. void build_fname( char *dest, char *s1, char *s2 )
  379. {
  380.     char *cptr;
  381.  
  382.     strcpy( dest, s1 );                 /* Pfad kopieren.                */
  383.     cptr = strrchr( dest, (int) BACKSLASH);
  384.     if (strrchr( s2, (int) '.')==0)
  385.         strcat(s2,".FTR");
  386.     strcpy( ++cptr, s2);                /* Schlie₧lich den Dateinamen    */
  387. }                                      /* dranhängen.                   */
  388. void save_filter(void)
  389. {
  390.     char s[256];
  391.     int handle,i,j;
  392.  
  393.     handle=(int)Fcreate(Name,0);
  394.     if (handle<0)
  395.     {
  396.         form_alert(1,"[3][Cannot open File][Cancel]");
  397.         return;
  398.     }
  399.     if(maintree[LOWPASS].ob_state == SELECTED)
  400.         sprintf(s,";Lowpass Filter, upper frequency: %.0f Hz.\r\n",upper_freq);
  401.     else    
  402.         if(maintree[HIGHPASS].ob_state == SELECTED)
  403.             sprintf(s,";Highpass Filter, lower frequency: %.0f Hz.\r\n",lower_freq);
  404.         else
  405.           if(maintree[BANDPASS].ob_state == SELECTED)
  406.                 sprintf(s,";Bandpass Filter, lower frequency: %.0f Hz. upper frequency: %.0f Hz.\r\n",lower_freq,upper_freq);
  407.             else
  408.               if(maintree[BANDSTOP].ob_state == SELECTED)
  409.                     sprintf(s,";Bandstop Filter, upper frequency: %.0f Hz. lower frequency: %.0f Hz.\r\n",upper_freq,lower_freq);
  410.     Fwrite(handle,strlen(s),s);
  411.     sprintf(s,";Sample frequency: %.0f Hz.\r\n;%d coefficients\r\n",smpl_freq,n_coeff);
  412.     Fwrite(handle,strlen(s),s);
  413.     if(maintree[BARTLETT].ob_state == SELECTED)
  414.         sprintf(s,";Using Bartlett Windowing\r\n");
  415.     else    
  416.         if(maintree[BLACKMAN].ob_state == SELECTED)
  417.             sprintf(s,";Using Blackman Windowing\r\n");
  418.         else    
  419.             if(maintree[FEJER].ob_state == SELECTED)
  420.                 sprintf(s,";Using Fejer Windowing\r\n");
  421.             else    
  422.                 if(maintree[HAMM].ob_state == SELECTED)
  423.                     sprintf(s,";Using Hamming Windowing\r\n");
  424.                 else
  425.                     if(maintree[HANN].ob_state == SELECTED)
  426.                         sprintf(s,";Using Hanning Windowing\r\n");
  427.                     else
  428.                         if(maintree[LANCZOS].ob_state == SELECTED)
  429.                             sprintf(s,";Using Lanczos Windowing\r\n");
  430.                         else    
  431.                             if(maintree[RECT].ob_state == SELECTED)        /* Default        */
  432.                                 sprintf(s,";No Windowing\r\n");
  433.     Fwrite(handle,strlen(s),s);
  434.     if (strrchr( Name, (int) BACKSLASH)!=0)
  435.         strcpy(s,strrchr( Name, (int) BACKSLASH)+1);
  436.     else
  437.         strcpy(s,Name);
  438.     if (strrchr( s, (int) '.')!=0)
  439.         strcpy(strrchr( s, (int) '.'),"\r\n");
  440.     else
  441.         strcat(s,"\r\n");
  442.     Fwrite(handle,strlen(s),s);
  443.     i=0;
  444.     while (i+10<=n_coeff)
  445.     {
  446.         sprintf(s,"        dc ");
  447.         Fwrite(handle,strlen(s),s);
  448.         for (j=0;j<9;j++)
  449.         {
  450.             sprintf(s,"$%02x%02x%02x,",(int)coeff[i][0] & 0xff,(int)coeff[i][1] & 0xff,(int)coeff[i][2] & 0xff);
  451.             Fwrite(handle,strlen(s),s);
  452.             i++;
  453.         }
  454.         sprintf(s,"$%02x%02x%02x\r\n",(int)coeff[i][0] & 0xff,(int)coeff[i][1] & 0xff,(int)coeff[i][2] & 0xff);
  455.         Fwrite(handle,strlen(s),s);
  456.         i++;
  457.     }
  458.     if (i<n_coeff)
  459.     {
  460.         sprintf(s,"        dc ");
  461.         Fwrite(handle,strlen(s),s);
  462.         if (i<n_coeff-1)
  463.         {
  464.             for (j=0;j<n_coeff-i-1;j++)
  465.             {
  466.                 sprintf(s,"$%02x%02x%02x,",(int)coeff[i+j][0] & 0xff,(int)coeff[i+j][1] & 0xff,(int)coeff[i+j][2] & 0xff);
  467.                 Fwrite(handle,strlen(s),s);
  468.             }
  469.         }
  470.         sprintf(s,"$%02x%02x%02x\r\n",(int)coeff[n_coeff-1][0] & 0xff,(int)coeff[n_coeff-1][1] & 0xff,(int)coeff[n_coeff-1][2] & 0xff);
  471.         Fwrite(handle,strlen(s),s);
  472.     }
  473.     Fclose(handle);
  474. }
  475.