-new pll code with hiltert filter

-new resample filter
-accept different sampling rates

dsp.c

@@ -27,26 +27,37 @@
 #include "filter.h"
 #include "filtercoeff.h"
 
-#define Fe 11025.0
 #define Fc 2400.0
 #define DFc 50.0
 #define PixelLine 2080
 #define Fp (2*PixelLine)
-#define RSMULT 10
+#define RSMULT 15
 #define Fi (Fp*RSMULT)
-static double FreqOsc = Fc / Fe;
+static double Fe;
+static double FreqOsc;
 static double FreqLine = 1.0;
+static double fr;
+static double offset = 0.0;
+
 
 extern int getsample(float *inbuff, int nb);
 
-static float pll(float In)
+int init_dsp(double F)
+{
+if(F > Fi) return (1);
+if(F < Fp) return (-1);
+Fe=F;
+fr=FreqOsc=Fc/Fe;
+return(0);
+}
+
+static float pll(double I, double Q)
 {
 
 /* pll coeff */
 #define K1 5e-3
 #define K2 3e-6
     static double PhaseOsc = 0.0;
-    static iirbuff_t Ifilterbuff, Qfilterbuff;
     double Io, Qo;
     double Ip, Qp;
     double DPhi;
@@ -57,8 +68,8 @@ static float pll(float In)
     Qo = sin(PhaseOsc);
 
 /* phase detector */
-    Ip = iir(In * Io, &Ifilterbuff, &PhaseFilterCf);
-    Qp = iir(In * Qo, &Qfilterbuff, &PhaseFilterCf);
+    Ip = I*Io-Q*Qo;
+    Qp = I*Qo+Q*Io;
     DPhi = -atan2(Qp, Ip) / M_PI;
 
 /*  loop filter  */
@@ -73,59 +84,75 @@ static float pll(float In)
 	PhaseOsc -= 2.0 * M_PI;
     if (PhaseOsc <= -M_PI)
 	PhaseOsc += 2.0 * M_PI;
-    return (float) (In * Io);
-}
 
-static double fr = Fc / Fe;
-static double offset = 0.0;
+    return ((float)Ip);
+}
 
-int getamp(float *ambuff, int nb)
+static int getamp(float *ambuff, int nb)
 {
 
 #define BLKIN 1024
-    float inbuff[BLKIN];
+    static float inbuff[BLKIN];
+    static int idxin=0;
+    static int nin=0;
+
     int n;
-    int res;
 
-    res = getsample(inbuff, nb > BLKIN ? BLKIN : nb);
-    for (n = 0; n < res; n++) {
-	ambuff[n] = pll(inbuff[n]);
+    for (n = 0; n < nb; n++) {
+    	double I,Q;
+
+	if (nin < IQFilterLen*2) {
+	    int res;
+	    memmove(inbuff, &(inbuff[idxin]), nin * sizeof(float));
+	    idxin = 0;
+    	    res = getsample(&(inbuff[nin]), BLKIN - nin);
+	    nin += res;
+	    if (nin < IQFilterLen*2)
+		return (n);
+	}
+
+    	iqfir(&inbuff[idxin],iqfilter,IQFilterLen,&I,&Q);
+	ambuff[n] = pll(I,Q);
 	fr = 0.25 * FreqOsc + 0.75 * fr;
+	idxin += 1;
+	nin -= 1;
     }
-    return (res);
+    return (n);
 }
 
 int getpixelv(float *pvbuff, int nb)
 {
 
-#define BLKAMP 256
+#define BLKAMP 1024
     static float ambuff[BLKAMP];
     static int nam = 0;
     static int idxam = 0;
-    int n;
+
+    int n,m;
+    double mult;
+
+    mult = (double) Fi *fr / Fc * FreqLine;
+
+    m=RSFilterLen/mult+1;
 
     for (n = 0; n < nb; n++) {
-	double mult;
 	int shift;
 
-	if (nam < BLKAMP) {
+	if (nam < m) {
 	    int res;
 	    memmove(ambuff, &(ambuff[idxam]), nam * sizeof(float));
 	    idxam = 0;
 	    res = getamp(&(ambuff[nam]), BLKAMP - nam);
 	    nam += res;
-	    if (nam < BLKAMP)
+	    if (nam < m)
 		return (n);
 	}
 
-	mult = (double) Fi *fr / Fc * FreqLine;
+	pvbuff[n] = rsfir(&(ambuff[idxam]), rsfilter, RSFilterLen, offset, mult) * mult * 2 * 256.0;
 
-	pvbuff[n] =
-	    rsfir(&(ambuff[idxam]), rsfilter, RSFilterLen, offset,
-		  mult) * mult * 2 * 256.0;
+	shift = ((int) floor((RSMULT - offset) / mult))+1;
+	offset = shift*mult+offset-RSMULT ;
 
-	shift = (int) ((RSMULT - offset + mult - 1) / mult);
-	offset = shift * mult + offset - RSMULT;
 	idxam += shift;
 	nam -= shift;
     }

filter.c

@@ -20,6 +20,7 @@
  *
  */
 #include "filter.h"
+#include <math.h>
 
 float fir(float *buff, const float *coeff, const int len)
 {
@@ -33,22 +34,18 @@ float fir(float *buff, const float *coeff, const int len)
     return r;
 }
 
-void
-iqfir(float *buff, const float *Icoeff, const float *Qcoeff, const int len,
-      float *Iptr, float *Qptr)
+void iqfir(float *buff, const float *coeff, const int len,double *I,double *Q)
 {
-    int i;
-    float I, Q;
-
-    I = Q = 0.0;
-    for (i = 0; i < len; i++) {
-	double v;
+    int k;
+    double i,q;
 
-	v = buff[i];
-	I += v * Icoeff[i];
-	Q += v * Qcoeff[i];
-    } *Iptr = I;
-    *Qptr = Q;
+    i=q=0.0;
+    for (k = 0; k < len; k++) {
+	i += buff[2*k] ;
+	q += buff[2*k] * coeff[k];
+    } 
+    i= buff[len-1]-i/len;
+    *I=i,*Q=q;
 }
 
 float
@@ -60,29 +57,14 @@ rsfir(float *buff, const float *coeff, const int len, const double offset,
     double out;
 
     out = 0.0;
-    for (i = 0, n = offset; n < len - 1; n += delta, i++) {
+    for (i = 0, n = offset; i < (len-1)/delta-1; n += delta, i++) {
 	int k;
 	double alpha;
 
-	k = (int) n;
+	k = (int)floor(n);
 	alpha = n - k;
-	out += buff[i] * (coeff[k] * (1.0 - alpha) + coeff[k + 1] * alpha);
-    } return out;
+	out += buff[i]*(coeff[k]*(1.0-alpha)+coeff[k + 1]*alpha);
+    }
+    return out;
 }
 
-double iir(double x, iirbuff_t * buff, const iircoeff_t * coeff)
-{
-    buff->x[4] = buff->x[3];
-    buff->x[3] = buff->x[2];
-    buff->x[2] = buff->x[1];
-    buff->x[1] = buff->x[0];
-    buff->x[0] = x / coeff->G;
-    buff->y[2] = buff->y[1];
-    buff->y[1] = buff->y[0];
-    buff->y[0] = buff->x[4]
-	+ coeff->x[2] * buff->x[3]
-	+ coeff->x[1] * buff->x[2]
-	+ coeff->x[0] * buff->x[1]
-	+ buff->x[0] + coeff->y[1] * buff->y[2] + coeff->y[0] * buff->y[1];
-    return (buff->y[0]);
-}

filter.h

@@ -20,19 +20,8 @@
  *
  */
 
-typedef struct {
-float G;
-float x[3];
-float y[2];
-} iircoeff_t;
 
-typedef struct {
-float x[5];
-float y[3];
-} iirbuff_t;
-
-float rsfir(float *buff,const float *coeff,const int len ,const double offset ,const double delta);
-void iqfir(float *buff,const float *Icoeff,const float *Qcoeff, const int len ,float *I, float *Q);
 float fir(float *buff,const float *coeff,const int len);
-double iir(double x,iirbuff_t *buff, const iircoeff_t *coeff);
+void  iqfir(float *buff,const float *coeff,const int len,double *I,double *Q);
+float rsfir(float *buff,const float *coeff,const int len ,const double offset ,const double delta);
 

filtercoeff.h

@@ -20,13 +20,18 @@
  *
  */
 
-const iircoeff_t PhaseFilterCf=
-{  3.106389554e+03,
-  { 3.8373714492, 5.6747428984, 3.8373714492 },
-  { 1.8770305106,-0.8819717729 }
+#define IQFilterLen 32
+const float iqfilter[IQFilterLen] = {
+-0.0205361, -0.0219524, -0.0235785, -0.0254648,
+-0.0276791, -0.0303152, -0.0335063, -0.0374482,
+-0.0424413, -0.0489708, -0.0578745, -0.0707355,
+-0.0909457, -0.127324, -0.212207, -0.63662,
+0.63662, 0.212207, 0.127324, 0.0909457,
+0.0707355, 0.0578745, 0.0489708, 0.0424413,
+0.0374482, 0.0335063, 0.0303152, 0.0276791,
+0.0254648, 0.0235785, 0.0219524, 0.0205361
 };
 
-
 #define SyncFilterLen 32
 const float Sync[SyncFilterLen]={
  -14,-14,-14,
@@ -36,98 +41,15 @@ const float Sync[SyncFilterLen]={
 };
 
 
-#define RSFilterLen 455
+#define RSFilterLen 75
 const float rsfilter[RSFilterLen] = {
--1.34249E-03, 1.80631E-03, 5.90669E-04, -9.41810E-05, -4.07485E-04,
--4.85881E-04, -4.25951E-04, -2.95340E-04, -1.40622E-04, 8.99745E-06,
- 1.36594E-04, 2.26986E-04, 2.85762E-04, 2.94814E-04, 2.77103E-04,
- 2.22380E-04, 1.42116E-04, 4.70086E-05, -5.48539E-05, -1.53666E-04,
--2.40057E-04, -3.05583E-04, -3.38473E-04, -3.31887E-04, -2.88025E-04,
--2.00097E-04, -7.80172E-05, 6.00740E-05, 2.04035E-04, 3.27028E-04,
- 4.09429E-04, 4.49753E-04, 4.42694E-04, 3.90744E-04, 2.91131E-04,
- 1.41069E-04, -5.08986E-05, -2.51913E-04, -4.13487E-04, -5.24003E-04,
--6.21400E-04, -6.34251E-04, -5.52247E-04, -4.21106E-04, -2.20212E-04,
- 1.86971E-05, 2.70810E-04, 5.06335E-04, 6.95368E-04, 8.12780E-04,
- 8.41294E-04, 7.74712E-04, 6.05319E-04, 3.65225E-04, 5.97767E-05,
--2.79576E-04, -5.85626E-04, -8.51135E-04, -1.04118E-03, -1.10967E-03,
--1.04530E-03, -8.59561E-04, -5.69134E-04, -1.96493E-04, 2.25062E-04,
- 6.43802E-04, 1.01013E-03, 1.27553E-03, 1.40162E-03, 1.37519E-03,
- 1.19236E-03, 8.58959E-04, 4.11732E-04, -1.08112E-04, -6.42232E-04,
--1.12853E-03, -1.50942E-03, -1.73155E-03, -1.76040E-03, -1.59382E-03,
--1.24024E-03, -7.13718E-04, -1.04045E-04, 5.64297E-04, 1.18859E-03,
- 1.70866E-03, 2.05610E-03, 2.18481E-03, 2.07002E-03, 1.71311E-03,
- 1.14477E-03, 4.21690E-04, -3.55578E-04, -1.25508E-03, -1.71599E-03,
--2.41463E-03, -2.69109E-03, -2.58592E-03, -2.22118E-03, -1.64517E-03,
--8.79739E-04, 2.69032E-05, 9.88114E-04, 1.89024E-03, 2.61089E-03,
- 3.06416E-03, 3.16887E-03, 2.92092E-03, 2.32859E-03, 1.45518E-03,
- 3.95690E-04, -7.36841E-04, -1.82279E-03, -2.74182E-03, -3.38572E-03,
--3.67163E-03, -3.56243E-03, -3.06396E-03, -2.20473E-03, -1.07168E-03,
- 2.11501E-04, 1.52499E-03, 2.71421E-03, 3.64024E-03, 4.20033E-03,
- 4.30452E-03, 3.92241E-03, 3.07838E-03, 1.86091E-03, 4.12894E-04,
--1.09233E-03, -2.49094E-03, -3.67748E-03, -4.59237E-03, -4.94819E-03,
--4.72195E-03, -4.04181E-03, -2.87213E-03, -1.35672E-03, 3.48096E-04,
- 2.05986E-03, 3.59116E-03, 4.77047E-03, 5.45966E-03, 5.57156E-03,
- 5.06959E-03, 4.01206E-03, 2.48309E-03, 6.38942E-04, -1.30369E-03,
--3.16818E-03, -4.74034E-03, -5.82407E-03, -6.30342E-03, -6.11965E-03,
--5.26980E-03, -3.82401E-03, -1.92513E-03, 2.24480E-04, 2.40210E-03,
- 4.37609E-03, 5.92750E-03, 6.88184E-03, 7.12199E-03, 6.59535E-03,
- 5.35465E-03, 3.50961E-03, 1.24232E-03, -1.21891E-03, -3.61467E-03,
--5.67831E-03, -7.18706E-03, -7.98927E-03, -7.96553E-03, -7.05584E-03,
--5.43074E-03, -3.14084E-03, -4.79904E-04, 2.31258E-03, 4.94032E-03,
- 7.12564E-03, 8.62696E-03, 9.26797E-03, 8.95857E-03, 7.70228E-03,
- 5.61491E-03, 2.82257E-03, -2.51761E-04, -3.54470E-03, -6.50595E-03,
--8.89239E-03, -1.04874E-02, -1.10720E-02, -1.05258E-02, -8.86513E-03,
--6.22409E-03, -2.83088E-03, 1.00897E-03, 4.94996E-03, 8.59207E-03,
- 1.15713E-02, 1.35495E-02, 1.42747E-02, 1.36027E-02, 1.15090E-02,
- 8.09566E-03, 3.60181E-03, -1.62033E-03, -7.13494E-03, -1.24571E-02,
--1.70701E-02, -2.04557E-02, -2.21780E-02, -2.18923E-02, -1.93358E-02,
--1.44250E-02, -7.21445E-03, 2.10551E-03, 1.31706E-02, 2.55127E-02,
- 3.85612E-02, 5.16835E-02, 6.42196E-02, 7.55240E-02, 8.50058E-02,
- 9.21659E-02, 9.66207E-02, 9.32814E-02, 9.66207E-02, 9.21659E-02,
- 8.50058E-02, 7.55240E-02, 6.42196E-02, 5.16835E-02, 3.85612E-02,
- 2.55127E-02, 1.31706E-02, 2.10551E-03, -7.21445E-03, -1.44250E-02,
--1.93358E-02, -2.18923E-02, -2.21780E-02, -2.04557E-02, -1.70701E-02,
--1.24571E-02, -7.13494E-03, -1.62033E-03, 3.60181E-03, 8.09566E-03,
- 1.15090E-02, 1.36027E-02, 1.42747E-02, 1.35495E-02, 1.15713E-02,
- 8.59207E-03, 4.94996E-03, 1.00897E-03, -2.83088E-03, -6.22409E-03,
--8.86513E-03, -1.05258E-02, -1.10720E-02, -1.04874E-02, -8.89239E-03,
--6.50595E-03, -3.54470E-03, -2.51761E-04, 2.82257E-03, 5.61491E-03,
- 7.70228E-03, 8.95857E-03, 9.26797E-03, 8.62696E-03, 7.12564E-03,
- 4.94032E-03, 2.31258E-03, -4.79904E-04, -3.14084E-03, -5.43074E-03,
--7.05584E-03, -7.96553E-03, -7.98927E-03, -7.18706E-03, -5.67831E-03,
--3.61467E-03, -1.21891E-03, 1.24232E-03, 3.50961E-03, 5.35465E-03,
- 6.59535E-03, 7.12199E-03, 6.88184E-03, 5.92750E-03, 4.37609E-03,
- 2.40210E-03, 2.24480E-04, -1.92513E-03, -3.82401E-03, -5.26980E-03,
--6.11965E-03, -6.30342E-03, -5.82407E-03, -4.74034E-03, -3.16818E-03,
--1.30369E-03, 6.38942E-04, 2.48309E-03, 4.01206E-03, 5.06959E-03,
- 5.57156E-03, 5.45966E-03, 4.77047E-03, 3.59116E-03, 2.05986E-03,
- 3.48096E-04, -1.35672E-03, -2.87213E-03, -4.04181E-03, -4.72195E-03,
--4.94819E-03, -4.59237E-03, -3.67748E-03, -2.49094E-03, -1.09233E-03,
- 4.12894E-04, 1.86091E-03, 3.07838E-03, 3.92241E-03, 4.30452E-03,
- 4.20033E-03, 3.64024E-03, 2.71421E-03, 1.52499E-03, 2.11501E-04,
--1.07168E-03, -2.20473E-03, -3.06396E-03, -3.56243E-03, -3.67163E-03,
--3.38572E-03, -2.74182E-03, -1.82279E-03, -7.36841E-04, 3.95690E-04,
- 1.45518E-03, 2.32859E-03, 2.92092E-03, 3.16887E-03, 3.06416E-03,
- 2.61089E-03, 1.89024E-03, 9.88114E-04, 2.69032E-05, -8.79739E-04,
--1.64517E-03, -2.22118E-03, -2.58592E-03, -2.69109E-03, -2.41463E-03,
--1.71599E-03, -1.25508E-03, -3.55578E-04, 4.21690E-04, 1.14477E-03,
- 1.71311E-03, 2.07002E-03, 2.18481E-03, 2.05610E-03, 1.70866E-03,
- 1.18859E-03, 5.64297E-04, -1.04045E-04, -7.13718E-04, -1.24024E-03,
--1.59382E-03, -1.76040E-03, -1.73155E-03, -1.50942E-03, -1.12853E-03,
--6.42232E-04, -1.08112E-04, 4.11732E-04, 8.58959E-04, 1.19236E-03,
- 1.37519E-03, 1.40162E-03, 1.27553E-03, 1.01013E-03, 6.43802E-04,
- 2.25062E-04, -1.96493E-04, -5.69134E-04, -8.59561E-04, -1.04530E-03,
--1.10967E-03, -1.04118E-03, -8.51135E-04, -5.85626E-04, -2.79576E-04,
- 5.97767E-05, 3.65225E-04, 6.05319E-04, 7.74712E-04, 8.41294E-04,
- 8.12780E-04, 6.95368E-04, 5.06335E-04, 2.70810E-04, 1.86971E-05,
--2.20212E-04, -4.21106E-04, -5.52247E-04, -6.34251E-04, -6.21400E-04,
--5.24003E-04, -4.13487E-04, -2.51913E-04, -5.08986E-05, 1.41069E-04,
- 2.91131E-04, 3.90744E-04, 4.42694E-04, 4.49753E-04, 4.09429E-04,
- 3.27028E-04, 2.04035E-04, 6.00740E-05, -7.80172E-05, -2.00097E-04,
--2.88025E-04, -3.31887E-04, -3.38473E-04, -3.05583E-04, -2.40057E-04,
--1.53666E-04, -5.48539E-05, 4.70086E-05, 1.42116E-04, 2.22380E-04,
- 2.77103E-04, 2.94814E-04, 2.85762E-04, 2.26986E-04, 1.36594E-04,
- 8.99745E-06, -1.40622E-04, -2.95340E-04, -4.25951E-04, -4.85881E-04,
--4.07485E-04, -9.41810E-05, 5.90669E-04, 1.80631E-03, -1.34249E-03
-};
-
+0.000684467, 0.000686301, 0.000680827, 0.000657135, 0.000598639, 0.000484572, 0.000292259, -4.07622e-19, 
+-0.000409636, -0.000946387, -0.00160904, -0.00238265, -0.00323652, -0.00412324, -0.00497894, -0.00572484, 
+-0.00627025, -0.00651673, -0.00636341, -0.00571307, -0.00447871, -0.00259011, 1.72476e-18, 0.00331064, 
+0.00732849, 0.0120054, 0.017258, 0.0229688, 0.0289897, 0.0351475, 0.0412507, 0.0470982, 
+0.052488, 0.0572273, 0.0611415, 0.0640834, 0.0659403, 0.0666398, 0.0661536, 0.0644993, 
+0.061739, 0.0579765, 0.0533519, 0.0480347, 0.0422149, 0.0360944, 0.0298768, 0.023758, 
+0.0179179, 0.0125126, 0.00766851, 0.00347853, 1.81998e-18, -0.00274526, -0.00476897, -0.00611273, 
+-0.00684295, -0.00704483, -0.00681579, -0.00625878, -0.00547594, -0.00456294, -0.00360441, -0.00267047, 
+-0.00181474, -0.00107366, -0.000467084, -4.66468e-19, 0.000334865, 0.000553877, 0.000679047, 0.000734606, 
+0.000743914, 0.000726905, 0.00069827 };

main.c

@@ -34,6 +34,7 @@
 #include "version.h"
 
 extern int getpixelrow(float *pixelv);
+extern int init_dsp(double F);;
 
 #define SYNC_WIDTH 39
 #define SPC_WIDTH 47
@@ -47,6 +48,7 @@ static SNDFILE *inwav;
 static int initsnd(char *filename)
 {
     SF_INFO infwav;
+    int	    res;
 
 /* open wav input file */
     infwav.format = 0;
@@ -55,14 +57,20 @@ static int initsnd(char *filename)
 	fprintf(stderr, "could not open %s\n", filename);
 	return (1);
     }
-    if (infwav.samplerate != 11025) {
-	fprintf(stderr, "Bad Input File sample rate: %d. Must be 11025\n",
-		infwav.samplerate);
+
+    res=init_dsp(infwav.samplerate);
+    if(res<0) {
+	fprintf(stderr, "Sample rate too low : %d\n", infwav.samplerate);
+	return (1);
+    }
+    if(res>0) {
+	fprintf(stderr, "Sample rate too hight : %d\n", infwav.samplerate);
 	return (1);
     }
+    fprintf(stderr, "Sample rate : %d\n", infwav.samplerate);
+
     if (infwav.channels != 1) {
-	fprintf(stderr, "Too many channels in input file : %d\n",
-		infwav.channels);
+	fprintf(stderr, "Too many channels in input file : %d\n", infwav.channels);
 	return (1);
     }
 

version.h

@@ -1,2 +1,2 @@
-const char version[]="Atpdec 1.7  (c) 2004-2005 Thierry Leconte F4DWV";
+const char version[]="Atpdec CVS version  (c) 2004-2005 Thierry Leconte F4DWV";