jDttSP/fm_demod.c

   1 /* fm_demod.c */
   2
   3 #include <fm_demod.h>
   4
   5 /*------------------------------------------------------------------------------*/
   6 /* private to FM */
   7 /*------------------------------------------------------------------------------*/
   8
   9 PRIVATE void
  10 init_pll(FMD fm,
  11          REAL samprate,
  12          REAL freq,
  13          REAL lofreq,
  14          REAL hifreq,
  15          REAL bandwidth) {
  16   REAL fac = TWOPI / samprate;
  17
  18   fm->pll.freq.f = freq * fac;
  19   fm->pll.freq.l = lofreq * fac;
  20   fm->pll.freq.h = hifreq * fac;
  21   fm->pll.phs = 0.0;
  22   fm->pll.delay = cxJ;
  23
  24   fm->pll.iir.alpha = bandwidth * fac; /* arm filter */
  25   fm->pll.alpha = fm->pll.iir.alpha * 0.3; /* pll bandwidth */
  26   fm->pll.beta = fm->pll.alpha * fm->pll.alpha * 0.25; /* second order term */
  27 }
  28
  29 PRIVATE void
  30 pll(FMD fm, COMPLEX sig) {
  31   COMPLEX z = Cmplx(cos(fm->pll.phs), sin(fm->pll.phs));
  32   REAL diff;
  33
  34   fm->pll.delay.re =  z.re * sig.re + z.im * sig.im;
  35   fm->pll.delay.im = -z.im * sig.re + z.re * sig.im;
  36   diff = ATAN2(fm->pll.delay.im, fm->pll.delay.re);
  37
  38   fm->pll.freq.f += fm->pll.beta * diff;
  39
  40   if (fm->pll.freq.f < fm->pll.freq.l)
  41     fm->pll.freq.f = fm->pll.freq.l;
  42   if (fm->pll.freq.f > fm->pll.freq.h)
  43     fm->pll.freq.f = fm->pll.freq.h;
  44
  45   fm->pll.phs += fm->pll.freq.f + fm->pll.alpha * diff;
  46
  47   while (fm->pll.phs >= TWOPI) fm->pll.phs -= TWOPI;
  48   while (fm->pll.phs < 0) fm->pll.phs += TWOPI;
  49 }
  50
  51 /*------------------------------------------------------------------------------*/
  52 /* public */
  53 /*------------------------------------------------------------------------------*/
  54
  55 void
  56 FMDemod(FMD fm) {
  57   int i;
  58   for (i = 0; i < CXBsize(fm->ibuf); i++) {
  59     pll(fm, CXBdata(fm->ibuf, i));
  60     fm->afc = 0.9999 * fm->afc + 0.0001 * fm->pll.freq.f;
  61     CXBreal(fm->obuf, i) =
  62       CXBimag(fm->obuf, i) = (fm->pll.freq.f - fm->afc) * fm->cvt;
  63   }
  64 }
  65
  66 FMD
  67 newFMD(REAL samprate,
  68        REAL f_initial,
  69        REAL f_lobound,
  70        REAL f_hibound,
  71        REAL f_bandwid,
  72        int size,
  73        COMPLEX *ivec,
  74        COMPLEX *ovec,
  75        char *tag) {
  76   FMD fm = (FMD) safealloc(1, sizeof(FMDDesc), tag);
  77
  78   fm->size = size;
  79   fm->ibuf = newCXB(size, ivec, tag);
  80   fm->obuf = newCXB(size, ovec, tag);
  81
  82   init_pll(fm,
  83            samprate,
  84            f_initial,
  85            f_lobound,
  86            f_hibound,
  87            f_bandwid);
  88
  89   fm->lock = 0.5;
  90   fm->afc = 0.0;
  91   fm->cvt = 0.45*samprate/(M_PI*f_bandwid);
  92
  93   return fm;
  94 }
  95
  96 void
  97 delFMD(FMD fm) {
  98   if (fm) {
  99     delCXB(fm->ibuf);
 100     delCXB(fm->obuf);
 101     safefree((char *) fm);
 102   }
 103 }