jDttSP/filter.c

   1 /* filter.c
   2 This file is part of a program that implements a Software-Defined Radio.
   3
   4 Copyright (C) 2004 by Frank Brickle, AB2KT and Bob McGwier, N4HY
   5
   6 This program is free software; you can redistribute it and/or modify
   7 it under the terms of the GNU General Public License as published by
   8 the Free Software Foundation; either version 2 of the License, or
   9 (at your option) any later version.
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 GNU General Public License for more details.
  15
  16 You should have received a copy of the GNU General Public License
  17 along with this program; if not, write to the Free Software
  18 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19
  20 The authors can be reached by email at
  21
  22 ab2kt@arrl.net
  23 or
  24 rwmcgwier@comcast.net
  25
  26 or by paper mail at
  27
  28 The DTTS Microwave Society
  29 6 Kathleen Place
  30 Bridgewater, NJ 08807
  31 */
  32
  33 #include <filter.h>
  34
  35 static REAL onepi = 3.141592653589793;
  36
  37 #define twopi TWOPI
  38
  39 RealFIR
  40 newFIR_REAL(int size, char *tag) {
  41   RealFIR p = (RealFIRDesc *) safealloc(1, sizeof(RealFIRDesc), tag);
  42   FIRcoef(p) = (REAL *) safealloc(size, sizeof(REAL), tag);
  43   FIRsize(p) = size;
  44   FIRtype(p) = FIR_Undef;
  45   FIRiscomplex(p) = FALSE;
  46   FIRfqlo(p) = FIRfqhi(p) = -1.0;
  47   return p;
  48 }
  49
  50 ComplexFIR
  51 newFIR_COMPLEX(int size, char *tag) {
  52   ComplexFIR p = (ComplexFIRDesc *) safealloc(1, sizeof(ComplexFIRDesc), tag);
  53   FIRcoef(p) = (COMPLEX *) safealloc(size, sizeof(COMPLEX), tag);
  54   FIRsize(p) = size;
  55   FIRtype(p) = FIR_Undef;
  56   FIRiscomplex(p) = TRUE;
  57   FIRfqlo(p) = FIRfqhi(p) = -1.0;
  58   return p;
  59 }
  60
  61 void
  62 delFIR_REAL(RealFIR p) {
  63   if (p) {
  64     delvec_REAL(FIRcoef(p));
  65     free((void *) p);
  66   }
  67 }
  68
  69 void
  70 delFIR_COMPLEX(ComplexFIR p) {
  71   if (p) {
  72     delvec_COMPLEX(FIRcoef(p));
  73     free((void *) p);
  74   }
  75 }
  76
  77 RealFIR
  78 newFIR_Lowpass_REAL(REAL cutoff, REAL sr, int size) {
  79   if ((cutoff < 0.0) || (cutoff > (sr / 2.0))) return 0;
  80   else if (size < 1) return 0;
  81   else {
  82     RealFIR p;
  83     REAL *h, *w, fc = cutoff / sr;
  84     int i, midpoint;
  85
  86     if (!(size & 01)) size++;
  87     midpoint = (size >> 01) | 01;
  88     p = newFIR_REAL(size, "newFIR_Lowpass_REAL");
  89     h = FIRcoef(p);
  90     w = newvec_REAL(size, "newFIR_Lowpass_REAL window");
  91     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
  92
  93     for (i = 1; i <= size; i++) {
  94       int j = i - 1;
  95       if (i != midpoint)
  96         h[j] = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
  97       else
  98         h[midpoint - 1] = 2.0 * fc;
  99     }
 100
 101     delvec_REAL(w);
 102     FIRtype(p) = FIR_Lowpass;
 103     return p;
 104   }
 105 }
 106
 107 ComplexFIR
 108 newFIR_Lowpass_COMPLEX(REAL cutoff, REAL sr, int size) {
 109   if ((cutoff < 0.0) || (cutoff > (sr / 2.0))) return 0;
 110   else if (size < 1) return 0;
 111   else {
 112     ComplexFIR p;
 113     COMPLEX *h;
 114     REAL *w, fc = cutoff / sr;
 115     int i, midpoint;
 116
 117     if (!(size & 01)) size++;
 118     midpoint = (size >> 01) | 01;
 119     p = newFIR_COMPLEX(size, "newFIR_Lowpass_COMPLEX");
 120     h = FIRcoef(p);
 121     w = newvec_REAL(size, "newFIR_Lowpass_REAL window");
 122     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 123
 124     for (i = 1; i <= size; i++) {
 125       int j = i - 1;
 126       if (i != midpoint)
 127         h[j].re = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 128       else
 129         h[midpoint - 1].re = 2.0 * fc;
 130     }
 131
 132     delvec_REAL(w);
 133     FIRtype(p) = FIR_Lowpass;
 134     return p;
 135   }
 136 }
 137
 138 RealFIR
 139 newFIR_Bandpass_REAL(REAL lo, REAL hi, REAL sr, int size) {
 140   if ((lo < 0.0) || (hi > (sr / 2.0)) || (hi <= lo)) return 0;
 141   else if (size < 1) return 0;
 142   else {
 143     RealFIR p;
 144     REAL *h, *w, fc, ff;
 145     int i, midpoint;
 146
 147     if (!(size & 01)) size++;
 148     midpoint = (size >> 01) | 01;
 149     p = newFIR_REAL(size, "newFIR_Bandpass_REAL");
 150     h = FIRcoef(p);
 151     w = newvec_REAL(size, "newFIR_Bandpass_REAL window");
 152     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 153
 154     lo /= sr, hi /= sr;
 155     fc = (hi - lo) / 2.0;
 156     ff = (lo + hi) * onepi;
 157
 158     for (i = 1; i <= size; i++) {
 159       int j = i - 1;
 160       if (i != midpoint)
 161         h[j] = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 162       else
 163         h[midpoint - 1] = 2.0 * fc;
 164       h[j] *= 2.0 * cos(ff * (i - midpoint));
 165     }
 166
 167     delvec_REAL(w);
 168     FIRtype(p) = FIR_Bandpass;
 169     return p;
 170   }
 171 }
 172
 173 ComplexFIR
 174 newFIR_Bandpass_COMPLEX(REAL lo, REAL hi, REAL sr, int size) {
 175   if ((lo < -(sr/2.0)) || (hi > (sr / 2.0)) || (hi <= lo)) return 0;
 176   else if (size < 1) return 0;
 177   else {
 178     ComplexFIR p;
 179     COMPLEX *h;
 180     REAL *w, fc, ff;
 181     int i, midpoint;
 182
 183     if (!(size & 01)) size++;
 184     midpoint = (size >> 01) | 01;
 185     p = newFIR_COMPLEX(size, "newFIR_Bandpass_COMPLEX");
 186     h = FIRcoef(p);
 187     w = newvec_REAL(size, "newFIR_Bandpass_COMPLEX window");
 188     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 189
 190     lo /= sr, hi /= sr;
 191     fc = (hi - lo) / 2.0;
 192     ff = (lo + hi) * onepi;
 193
 194     for (i = 1; i <= size; i++) {
 195       int j = i - 1, k = i - midpoint;
 196       REAL tmp, phs = ff * k;
 197       if (i != midpoint)
 198         tmp = (sin(twopi * k * fc) / (onepi * k)) * w[j];
 199       else
 200         tmp = 2.0 * fc;
 201       tmp *= 2.0;
 202       h[j].re = tmp * cos(phs);
 203       h[j].im = tmp * sin(phs);
 204     }
 205
 206     delvec_REAL(w);
 207     FIRtype(p) = FIR_Bandpass;
 208     return p;
 209   }
 210 }
 211
 212 RealFIR
 213 newFIR_Highpass_REAL(REAL cutoff, REAL sr, int size) {
 214   if ((cutoff < 0.0) || (cutoff > (sr / 2.0))) return 0;
 215   else if (size < 1) return 0;
 216   else {
 217     RealFIR p;
 218     REAL *h, *w, fc = cutoff / sr;
 219     int i, midpoint;
 220
 221     if (!(size & 01)) size++;
 222     midpoint = (size >> 01) | 01;
 223     p = newFIR_REAL(size, "newFIR_Highpass_REAL");
 224     h = FIRcoef(p);
 225     w = newvec_REAL(size, "newFIR_Highpass_REAL window");
 226     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 227
 228     for (i = 1; i <= size; i++) {
 229       int j = i - 1;
 230       if (i != midpoint)
 231         h[j] = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 232       else
 233         h[midpoint - 1] = 2.0 * fc;
 234     }
 235
 236     for (i = 1; i <= size; i++) {
 237       int j = i - 1;
 238       if (i != midpoint) h[j] = -h[j];
 239       else h[midpoint - 1] = 1.0 - h[midpoint - 1];
 240     }
 241
 242     delvec_REAL(w);
 243     FIRtype(p) = FIR_Highpass;
 244     return p;
 245   }
 246 }
 247
 248 ComplexFIR
 249 newFIR_Highpass_COMPLEX(REAL cutoff, REAL sr, int size) {
 250   if ((cutoff < 0.0) || (cutoff > (sr / 2.0))) return 0;
 251   else if (size < 1) return 0;
 252   else {
 253     ComplexFIR p;
 254     COMPLEX *h;
 255     REAL *w, fc = cutoff / sr;
 256     int i, midpoint;
 257
 258     if (!(size & 01)) size++;
 259     midpoint = (size >> 01) | 01;
 260     p = newFIR_COMPLEX(size, "newFIR_Highpass_REAL");
 261     h = FIRcoef(p);
 262     w = newvec_REAL(size, "newFIR_Highpass_REAL window");
 263     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 264
 265     for (i = 1; i <= size; i++) {
 266       int j = i - 1;
 267       if (i != midpoint)
 268         h[j].re = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 269       else
 270         h[midpoint - 1].re = 2.0 * fc;
 271     }
 272
 273     for (i = 1; i <= size; i++) {
 274       int j = i - 1;
 275       if (i != midpoint) h[j].re = -h[j].re;
 276       else h[midpoint - 1].re = 1.0 - h[midpoint - 1].re;
 277     }
 278
 279     delvec_REAL(w);
 280     FIRtype(p) = FIR_Highpass;
 281     return p;
 282   }
 283 }
 284
 285 RealFIR
 286 newFIR_Hilbert_REAL(REAL lo, REAL hi, REAL sr, int size) {
 287   if ((lo < 0.0) || (hi > (sr / 2.0)) || (hi <= lo)) return 0;
 288   else if (size < 1) return 0;
 289   else {
 290     RealFIR p;
 291     REAL *h, *w, fc, ff;
 292     int i, midpoint;
 293
 294     if (!(size & 01)) size++;
 295     midpoint = (size >> 01) | 01;
 296     p = newFIR_REAL(size, "newFIR_Hilbert_REAL");
 297     h = FIRcoef(p);
 298     w = newvec_REAL(size, "newFIR_Hilbert_REAL window");
 299     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 300
 301     lo /= sr, hi /= sr;
 302     fc = (hi - lo) / 2.0;
 303     ff = (lo + hi) * onepi;
 304
 305     for (i = 1; i <= size; i++) {
 306       int j = i - 1;
 307       if (i != midpoint)
 308         h[j] = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 309       else
 310         h[midpoint - 1] = 2.0 * fc;
 311       h[j] *= 2.0 * sin(ff * (i - midpoint));
 312     }
 313
 314     delvec_REAL(w);
 315     FIRtype(p) = FIR_Hilbert;
 316     return p;
 317   }
 318 }
 319
 320 ComplexFIR
 321 newFIR_Hilbert_COMPLEX(REAL lo, REAL hi, REAL sr, int size) {
 322   if ((lo < 0.0) || (hi > (sr / 2.0)) || (hi <= lo)) return 0;
 323   else if (size < 1) return 0;
 324   else {
 325     ComplexFIR p;
 326     COMPLEX *h;
 327     REAL *w, fc, ff;
 328     int i, midpoint;
 329
 330     if (!(size & 01)) size++;
 331     midpoint = (size >> 01) | 01;
 332     p = newFIR_COMPLEX(size, "newFIR_Hilbert_COMPLEX");
 333     h = FIRcoef(p);
 334     w = newvec_REAL(size, "newFIR_Hilbert_COMPLEX window");
 335     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 336
 337     lo /= sr, hi /= sr;
 338     fc = (hi - lo) / 2.0;
 339     ff = (lo + hi) * onepi;
 340
 341     for (i = 1; i <= size; i++) {
 342       int j = i - 1;
 343       REAL tmp, phs = ff * (i - midpoint);
 344       if (i != midpoint)
 345         tmp = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 346       else
 347         tmp = 2.0 * fc;
 348       tmp *= 2.0;
 349       /* h[j].re *= tmp * cos(phs); */
 350       h[j].im *= tmp * sin(phs);
 351     }
 352
 353     delvec_REAL(w);
 354     FIRtype(p) = FIR_Hilbert;
 355     return p;
 356   }
 357 }
 358
 359 RealFIR
 360 newFIR_Bandstop_REAL(REAL lo, REAL hi, REAL sr, int size) {
 361   if ((lo < 0.0) || (hi > (sr / 2.0)) || (hi <= lo)) return 0;
 362   else if (size < 1) return 0;
 363   else {
 364     RealFIR p;
 365     REAL *h, *w, fc, ff;
 366     int i, midpoint;
 367
 368     if (!(size & 01)) size++;
 369     midpoint = (size >> 01) | 01;
 370     p = newFIR_REAL(size, "newFIR_Bandstop_REAL");
 371     h = FIRcoef(p);
 372     w = newvec_REAL(size, "newFIR_Bandstop_REAL window");
 373     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 374
 375     lo /= sr, hi /= sr;
 376     fc = (hi - lo) / 2.0;
 377     ff = (lo + hi) * onepi;
 378
 379     for (i = 1; i <= size; i++) {
 380       int j = i - 1;
 381       if (i != midpoint)
 382         h[j] = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 383       else
 384         h[midpoint - 1] = 2.0 * fc;
 385       h[j] *= 2.0 * cos(ff * (i - midpoint));
 386     }
 387
 388     for (i = 1; i <= size; i++) {
 389       int j = i - 1;
 390       if (i != midpoint) h[j] = -h[j];
 391       else h[midpoint - 1] = 1.0 - h[midpoint - 1];
 392     }
 393
 394     delvec_REAL(w);
 395     FIRtype(p) = FIR_Bandstop;
 396     return p;
 397   }
 398 }
 399
 400 ComplexFIR
 401 newFIR_Bandstop_COMPLEX(REAL lo, REAL hi, REAL sr, int size) {
 402   if ((lo < 0.0) || (hi > (sr / 2.0)) || (hi <= lo)) return 0;
 403   else if (size < 1) return 0;
 404   else {
 405     ComplexFIR p;
 406     COMPLEX *h;
 407     REAL *w, fc, ff;
 408     int i, midpoint;
 409
 410     if (!(size & 01)) size++;
 411     midpoint = (size >> 01) | 01;
 412     p = newFIR_COMPLEX(size, "newFIR_Bandstop_REAL");
 413     h = FIRcoef(p);
 414     w = newvec_REAL(size, "newFIR_Bandstop_REAL window");
 415     (void) makewindow(BLACKMANHARRIS_WINDOW, size, w);
 416
 417     lo /= sr, hi /= sr;
 418     fc = (hi - lo) / 2.0;
 419     ff = (lo + hi) * onepi;
 420
 421     for (i = 1; i <= size; i++) {
 422       int j = i - 1;
 423       REAL tmp, phs = ff * (i - midpoint);
 424       if (i != midpoint)
 425         tmp = (sin(twopi * (i - midpoint) * fc) / (onepi * (i - midpoint))) * w[j];
 426       else
 427         tmp = 2.0 * fc;
 428       tmp *= 2.0;
 429       h[j].re *= tmp * cos(phs);
 430       h[j].im *= tmp * sin(phs);
 431     }
 432
 433     for (i = 1; i <= size; i++) {
 434       int j = i - 1;
 435       if (i != midpoint) h[j] = Cmul(h[j], cxminusone);
 436       else h[midpoint - 1] = Csub(cxone, h[midpoint - 1]);
 437     }
 438
 439     delvec_REAL(w);
 440     FIRtype(p) = FIR_Bandstop;
 441     return p;
 442   }
 443 }
 444
 445 #ifdef notdef
 446 int
 447 main(int argc, char **argv) {
 448   int i, size = 101;
 449   RealFIR filt;
 450 #ifdef notdef
 451   filt = newFIR_Lowpass_REAL(1000.0, 48000.0, size);
 452   for (i = 0; i < FIRsize(filt); i++)
 453     printf("%d %f\n", i, FIRtap(filt, i));
 454   delFIR_REAL(filt);
 455 #endif
 456
 457 #ifdef notdef
 458   filt = newFIR_Bandstop_REAL(1000.0, 2000.0, 48000.0, size);
 459   for (i = 0; i < FIRsize(filt); i++)
 460     printf("%d %f\n", i, FIRtap(filt, i));
 461   delFIR_REAL(filt);
 462 #endif
 463
 464   filt = newFIR_Bandpass_REAL(1000.0, 2000.0, 48000.0, size);
 465   for (i = 0; i < FIRsize(filt); i++)
 466     printf("%d %f\n", i, FIRtap(filt, i));
 467   delFIR_REAL(filt);
 468
 469 #ifdef notdef
 470   filt = newFIR_Highpass_REAL(1000.0, 48000.0, size);
 471   for (i = 0; i < FIRsize(filt); i++)
 472     printf("%d %f\n", i, FIRtap(filt, i));
 473   delFIR_REAL(filt);
 474 #endif
 475
 476 #ifdef notdef
 477   filt = newFIR_Hilbert_REAL(1000.0, 2000.0, 48000.0, size);
 478   for (i = 0; i < FIRsize(filt); i++)
 479     printf("%d %f\n", i, FIRtap(filt, i));
 480   delFIR_REAL(filt);
 481 #endif
 482
 483 #ifdef notdef
 484   {
 485     COMPLEX *z;
 486     REAL *ttbl;
 487     int fftlen;
 488     fftlen = nblock2(size) * 2;
 489     z = newvec_COMPLEX(fftlen, "z");
 490     ttbl = newvec_REAL(fftlen, "ttbl");
 491     cfftm(FFT_INIT, fftlen, (float *) z, (float *) z, ttbl);
 492     for (i = 0; i < FIRsize(filt); i++)
 493       z[i].re = FIRtap(filt, i);
 494     cfftm(FFT_FORWARD, fftlen, (float *) z, (float *) z, ttbl);
 495     for (i = 0; i < size; i++) {
 496       printf("%d %f\n", i, Cabs(z[i]));
 497       delvec_COMPLEX(z);
 498       delvec_REAL(ttbl);
 499     }
 500   }
 501 #endif
 502   exit(0);
 503 }
 504 #endif