jDttSP/am_demod.c

   1 /* am_demod.c */
   2
   3 #include <am_demod.h>
   4 #include <cxops.h>
   5
   6 /*------------------------------------------------------------------------------*/
   7 /* private to AM */
   8 /*------------------------------------------------------------------------------*/
   9
  10 static void
  11 init_pll(AMD am,
  12          REAL samprate,
  13          REAL freq,
  14          REAL lofreq,
  15          REAL hifreq,
  16          REAL bandwidth) {
  17   REAL fac = TWOPI / samprate;
  18   am->pll.freq.f = freq * fac;
  19   am->pll.freq.l = lofreq * fac;
  20   am->pll.freq.h = hifreq * fac;
  21   am->pll.phs = 0.0;
  22   am->pll.delay = cxJ;
  23
  24   am->pll.iir.alpha = bandwidth * fac; /* arm filter */
  25   am->pll.alpha = am->pll.iir.alpha * 0.3;   /* pll bandwidth */
  26   am->pll.beta = am->pll.alpha * am->pll.alpha * 0.25; /* second order term */
  27   am->pll.fast_alpha = am->pll.alpha;
  28 }
  29
  30 static void
  31 pll(AMD am, COMPLEX sig) {
  32   COMPLEX z = Cmplx(cos(am->pll.phs), sin(am->pll.phs));
  33   REAL diff;
  34
  35   am->pll.delay.re =  z.re * sig.re + z.im * sig.im;
  36   am->pll.delay.im = -z.im * sig.re + z.re * sig.im;
  37   diff = Cmag(sig) * ATAN2(am->pll.delay.im, am->pll.delay.re);
  38
  39   am->pll.freq.f += am->pll.beta * diff;
  40
  41   if (am->pll.freq.f < am->pll.freq.l)
  42     am->pll.freq.f = am->pll.freq.l;
  43   if (am->pll.freq.f > am->pll.freq.h)
  44     am->pll.freq.f = am->pll.freq.h;
  45
  46   am->pll.phs += am->pll.freq.f + am->pll.alpha * diff;
  47
  48   while (am->pll.phs >= TWOPI) am->pll.phs -= TWOPI;
  49   while (am->pll.phs < 0) am->pll.phs += TWOPI;
  50 }
  51
  52 static double
  53 dem(AMD am) {
  54   am->lock.curr = 0.999 * am->lock.curr + 0.001 * fabs(am->pll.delay.im);
  55
  56   /* env collapse? */
  57  /* if ((am->lock.curr < 0.05) && (am->lock.prev >= 0.05)) {
  58     am->pll.alpha = 0.1 * am->pll.fast_alpha;
  59     am->pll.beta = am->pll.alpha * am->pll.alpha * 0.25;
  60   } else if ((am->pll.alpha > 0.05) && (am->lock.prev <= 0.05)) {
  61           am->pll.alpha = am->pll.fast_alpha;
  62   }*/
  63   am->lock.prev = am->lock.curr;
  64   am->dc = 0.99*am->dc + 0.01*am->pll.delay.re;
  65   return am->pll.delay.re-am->dc;
  66 }
  67
  68 /*------------------------------------------------------------------------------*/
  69 /* public */
  70 /*------------------------------------------------------------------------------*/
  71
  72 void
  73 AMDemod(AMD am) {
  74   int i;
  75   double demout;
  76   switch (am->mode) {
  77   case SAMdet:
  78     for (i = 0; i < am->size; i++) {
  79       pll(am, CXBdata(am->ibuf, i));
  80       demout = dem(am);
  81       CXBdata(am->obuf, i) = Cmplx(demout, demout);
  82     }
  83     break;
  84   case AMdet:
  85     for (i = 0; i < am->size; i++) {
  86       am->lock.curr = Cmag(CXBdata(am->ibuf, i));
  87       am->dc = 0.9999 * am->dc + 0.0001 * am->lock.curr;
  88       am->smooth = 0.5 * am->smooth + 0.5 * (am->lock.curr - am->dc);
  89       /* demout = am->smooth; */
  90       CXBdata(am->obuf, i) = Cmplx(am->smooth, am->smooth);
  91     }
  92     break;
  93   }
  94 }
  95
  96 AMD
  97 newAMD(REAL samprate,
  98        REAL f_initial,
  99        REAL f_lobound,
 100        REAL f_hibound,
 101        REAL f_bandwid,
 102        int size,
 103        COMPLEX *ivec,
 104        COMPLEX *ovec,
 105        AMMode mode,
 106        char *tag) {
 107   AMD am = (AMD) safealloc(1, sizeof(AMDDesc), tag);
 108
 109   am->size = size;
 110   am->ibuf = newCXB(size, ivec, tag);
 111   am->obuf = newCXB(size, ovec, tag);
 112   am->mode = mode;
 113   init_pll(am,
 114            samprate,
 115            f_initial,
 116            f_lobound,
 117            f_hibound,
 118            f_bandwid);
 119
 120   am->lock.curr = 0.5;
 121   am->lock.prev = 1.0;
 122   am->dc = 0.0;
 123
 124   return am;
 125 }
 126
 127 void
 128 delAMD(AMD am) {
 129   if (am) {
 130     delCXB(am->ibuf);
 131     delCXB(am->obuf);
 132     safefree((char *) am);
 133   }
 134 }