git.gag.com Git - fw/tmflights/blob - filter.5c

   1 /*
   2  * Kaiser Window digital filter
   3  *
   4  * Copyright © 2001, Keith Packard
   5  * All Rights Reserved.  See the file COPYING in this directory
   6  * for licensing information.
   7  */
   8
   9 real i0(real x)
  10 {
  11     real    ds, d, s;
  12
  13     ds = 1;
  14     d = 0;
  15     s = 0;
  16     do
  17     {
  18         d = d + 2;
  19         ds = ds * x**2 / d**2;
  20         s = s + ds;
  21 #       printf ("ds %g s %g dist %g\n", ds, s,
  22 #               ds - 0.2e-8 * s);
  23     }
  24     while (ds - 0.2e-8 * s > 0);
  25     return s;
  26 }
  27
  28 real highpass (real n, real m, real wc)
  29 {
  30     real  alpha = m/2;
  31     real  dist;
  32
  33     dist = n - alpha;
  34     if (dist == 0)
  35         return (pi/2 - wc) / pi;
  36     return -sin(dist * (pi/2-wc)) / (pi * dist);
  37 }
  38
  39 real lowpass (real n, real m, real wc)
  40 {
  41     real  alpha = m/2;
  42     real  dist;
  43     dist = n - alpha;
  44     if (dist == 0)
  45         return wc / pi;
  46     return sin (wc * dist) / (pi * dist);
  47 }
  48
  49 real kaiser (real n, real m, real beta)
  50 {
  51     real alpha = m / 2;
  52     return i0 (beta * sqrt (1 - ((n - alpha) / alpha)**2)) / i0(beta);
  53 }
  54
  55 void write_high (string filename,
  56                        real m,
  57                        real wc,
  58                        real beta,
  59                        real step)
  60 {
  61     real    n;
  62     file    f;
  63
  64     f = File::open (filename, "w");
  65     for (n = 0; n <= m; n += step)
  66         File::fprintf (f, "%g,\n", highpass (n, m, wc) * kaiser (n, m, beta));
  67     File::close (f);
  68 }
  69
  70 void write_low (string filename,
  71                     real m,
  72                     real wc,
  73                     real beta,
  74                     real step)
  75 {
  76     real    n;
  77     file    f;
  78
  79     f = File::open (filename, "w");
  80     for (n = 0; n <= m; n += step)
  81         File::fprintf (f, "%g,\n", lowpass (n, m, wc) * kaiser (n, m, beta));
  82     File::close (f);
  83 }
  84
  85 real β (real A)
  86 {
  87     if (A > 50)
  88         return 0.1102 * (A - 8.7);
  89     else if (A >= 21)
  90         return 0.5842 * (A - 21) ** 0.4 + 0.07886 * (A - 21);
  91     else
  92         return 0.0;
  93 }
  94
  95 int M (real A, real Δω)
  96 {
  97     if (A > 21)
  98         return ceil ((A - 7.95) / (2.285 * Δω));
  99     else
 100         return ceil(5.79 / Δω);
 101 }
 102
 103 typedef struct {
 104     real ωpass;
 105     real Δω;
 106     real A;
 107     real β;
 108     int M;
 109 } filter_param_t;
 110
 111 filter_param_t filter (real ωpass, real ωstop, real error)
 112 {
 113     filter_param_t  p;
 114     p.ωpass = ωpass;
 115     p.Δω = ωstop - ωpass;
 116     p.A = -20 * log10 (error);
 117     p.β = β (p.A);
 118     p.M = M (p.A, p.Δω);
 119     if ((p.M & 1) == 1)
 120         p.M++;
 121     return p;
 122 }
 123
 124 real[] low_pass_filter(real ωpass, real ωstop, real error)
 125 {
 126     filter_param_t  p = filter(ωpass, ωstop, error);
 127
 128     return (real[p.M + 1]) {
 129         [n] = lowpass(n, p.M, ωpass) * kaiser(n, p.M, p.β)
 130     };
 131 }
 132
 133