diff --git a/dsp.c b/dsp.c index c6c3694..79cc3bd 100755 --- a/dsp.c +++ b/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; } diff --git a/filter.c b/filter.c index 86e79c7..fab50cb 100755 --- a/filter.c +++ b/filter.c @@ -20,6 +20,7 @@ * */ #include "filter.h" +#include 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]); -} diff --git a/filter.h b/filter.h index 4aa4e03..12e2ad1 100755 --- a/filter.h +++ b/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); diff --git a/filtercoeff.h b/filtercoeff.h index 4783b27..57d862d 100755 --- a/filtercoeff.h +++ b/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 }; diff --git a/main.c b/main.c index be111bd..6f56c38 100644 --- a/main.c +++ b/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); } diff --git a/version.h b/version.h index 51c49f7..4fcc069 100644 --- a/version.h +++ b/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";