5 /*------------------------------------------------------------------------------*/
7 /*------------------------------------------------------------------------------*/
16 REAL fac = TWOPI / samprate;
18 fm->pll.freq.f = freq * fac;
19 fm->pll.freq.l = lofreq * fac;
20 fm->pll.freq.h = hifreq * fac;
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 */
30 pll(FMD fm, COMPLEX sig) {
31 COMPLEX z = Cmplx(cos(fm->pll.phs), sin(fm->pll.phs));
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);
38 fm->pll.freq.f += fm->pll.beta * diff;
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;
45 fm->pll.phs += fm->pll.freq.f + fm->pll.alpha * diff;
47 while (fm->pll.phs >= TWOPI) fm->pll.phs -= TWOPI;
48 while (fm->pll.phs < 0) fm->pll.phs += TWOPI;
51 /*------------------------------------------------------------------------------*/
53 /*------------------------------------------------------------------------------*/
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;
76 FMD fm = (FMD) safealloc(1, sizeof(FMDDesc), tag);
79 fm->ibuf = newCXB(size, ivec, tag);
80 fm->obuf = newCXB(size, ovec, tag);
91 fm->cvt = 0.45*samprate/(M_PI*f_bandwid);
101 safefree((char *) fm);