]> git.rkrishnan.org Git - dttsp.git/blob - jDttSP/fm_demod.c
Flushing the missing meter.c
[dttsp.git] / 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 }