jDttSP/dcblock.c

   1 // dcblock.h
   2 /*
   3 This file is part of a program that implements a Software-Defined Radio.
   4
   5 Copyright (C) 2004-2005 by Frank Brickle, AB2KT and Bob McGwier, N4HY
   6
   7 This program is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 This program is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with this program; if not, write to the Free Software
  19 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  20
  21 The authors can be reached by email at
  22
  23 ab2kt@arrl.net
  24 or
  25 rwmcgwier@comcast.net
  26
  27 or by paper mail at
  28
  29 The DTTS Microwave Society
  30 6 Kathleen Place
  31 Bridgewater, NJ 08807
  32 */
  33
  34 #include <dcblock.h>
  35
  36 // NB may have to ramify this a little
  37 // for other sampling rates; maybe not
  38
  39 PRIVATE REAL
  40 butterworth_hipass_100_2(REAL xin, REAL *xv, REAL *yv),
  41 butterworth_hipass_100_4(REAL xin, REAL *xv, REAL *yv),
  42 butterworth_hipass_100_6(REAL xin, REAL *xv, REAL *yv),
  43 butterworth_hipass_100_8(REAL xin, REAL *xv, REAL *yv);
  44
  45 void
  46 DCBlock(DCBlocker dcb) {
  47   int i;
  48   REAL x, y;
  49
  50   switch (dcb->lev) {
  51
  52   case DCB_LOW:
  53     for (i = 0; i < CXBsize(dcb->buf); i++) {
  54       x = CXBreal(dcb->buf, i);
  55       y = butterworth_hipass_100_2(x, dcb->old.inp, dcb->old.out);
  56       CXBdata(dcb->buf, i) = Cmplx(y, 0.0);
  57     }
  58     break;
  59
  60   case DCB_MED:
  61     for (i = 0; i < CXBsize(dcb->buf); i++) {
  62       x = CXBreal(dcb->buf, i);
  63       y = butterworth_hipass_100_4(x, dcb->old.inp, dcb->old.out);
  64       CXBdata(dcb->buf, i) = Cmplx(y, 0.0);
  65     }
  66     break;
  67
  68   case DCB_HIGH:
  69     for (i = 0; i < CXBsize(dcb->buf); i++) {
  70       x = CXBreal(dcb->buf, i);
  71       y = butterworth_hipass_100_6(x, dcb->old.inp, dcb->old.out);
  72       CXBdata(dcb->buf, i) = Cmplx(y, 0.0);
  73     }
  74     break;
  75
  76   case DCB_SUPER:
  77     for (i = 0; i < CXBsize(dcb->buf); i++) {
  78       x = CXBreal(dcb->buf, i);
  79       y = butterworth_hipass_100_8(x, dcb->old.inp, dcb->old.out);
  80       CXBdata(dcb->buf, i) = Cmplx(y, 0.0);
  81     }
  82     break;
  83
  84   default:
  85     break;
  86   }
  87 }
  88
  89 void
  90 resetDCBlocker(DCBlocker dcb, int lev) {
  91   memset((char *) dcb->old.inp, 0, BLKMEM * sizeof(REAL));
  92   memset((char *) dcb->old.out, 0, BLKMEM * sizeof(REAL));
  93   dcb->lev = lev;
  94 }
  95
  96 DCBlocker
  97 newDCBlocker(int lev, CXB buf) {
  98   DCBlocker dcb =
  99     (DCBlocker) safealloc(1, sizeof(DCBlockerInfo), "DCBlocker");
 100   dcb->buf = newCXB(CXBsize(buf), CXBbase(buf), "DCBlocker");
 101   dcb->lev = lev;
 102   return dcb;
 103 }
 104
 105 void
 106 delDCBlocker(DCBlocker dcb) {
 107   if (dcb) {
 108     delCXB(dcb->buf);
 109     safefree((char *) dcb);
 110   }
 111 }
 112
 113 // f == 0.002083 == 100 Hz at 48k
 114
 115 PRIVATE REAL
 116 butterworth_hipass_100_2(REAL xin, REAL *xv, REAL *yv) {
 117   int i;
 118
 119   for (i = 1; i < 2; i++)
 120     xv[i - 1] = xv[i], yv[i - 1] = yv[i];
 121
 122   xv[2] = xin / 1.009297482;
 123
 124   yv[2] =             (xv[0] + xv[2])
 125     + -2.0          *  xv[1]
 126     + -0.9816611902 *  yv[0]
 127     +  1.9814914708 *  yv[1];
 128
 129   return yv[2];
 130 }
 131
 132 PRIVATE REAL
 133 butterworth_hipass_100_4(REAL xin, REAL *xv, REAL *yv) {
 134   int i;
 135
 136   for (i = 1; i < 4; i++)
 137     xv[i - 1] = xv[i], yv[i - 1] = yv[i];
 138
 139   xv[4] = xin / 1.017247322;
 140
 141   yv[4] =             (xv[0] + xv[4])
 142     + -4.0          * (xv[1] + xv[3])
 143     +  6.0          *  xv[2]
 144     + -0.9663776767 *  yv[0]
 145     +  3.8985609655 *  yv[1]
 146     + -5.8979831706 *  yv[2]
 147     +  3.9657998529 *  yv[3];
 148
 149   return yv[4];
 150 }
 151
 152 PRIVATE REAL
 153 butterworth_hipass_100_6(REAL xin, REAL *xv, REAL *yv) {
 154   int i;
 155
 156   for (i = 1; i < 6; i++)
 157     xv[i - 1] = xv[i], yv[i - 1] = yv[i];
 158
 159   xv[6] = xin / 1.025606415;
 160
 161   yv[6] =              (xv[0] + xv[6])
 162     +  -6.0          * (xv[1] + xv[5])
 163     +  15.0          * (xv[2] + xv[4])
 164     + -20.0          *  xv[3]
 165     +  -0.9506891622 *  yv[0]
 166     +   5.7522090378 *  yv[1]
 167     + -14.5019247580 *  yv[2]
 168     +  19.4994114580 *  yv[3]
 169     + -14.7484389800 *  yv[4]
 170     +   5.9494324049 *  yv[5];
 171
 172   return yv[6];
 173 }
 174
 175 PRIVATE REAL
 176 butterworth_hipass_100_8(REAL xin, REAL *xv, REAL *yv) {
 177   int i;
 178
 179   for (i = 1; i < 8; i++)
 180     xv[i - 1] = xv[i], yv[i - 1] = yv[i];
 181
 182   xv[8] = xin / 1.034112352;
 183
 184   yv[8] =              (xv[0] + xv[8])
 185     +  -8.0          * (xv[1] + xv[7])
 186     +  28.0          * (xv[2] + xv[6])
 187     + -56.0          * (xv[3] + xv[5])
 188     +  70.0          *  xv[4]
 189     +  -0.9351139781 *  yv[0]
 190     +   7.5436450525 *  yv[1]
 191     + -26.6244301320 *  yv[2]
 192     +  53.6964633920 *  yv[3]
 193     + -67.6854640540 *  yv[4]
 194     +  54.6046308830 *  yv[5]
 195     + -27.5326449810 *  yv[6]
 196     +   7.9329138172 *  yv[7];
 197
 198   return yv[8];
 199 }