aptdec/dsp.c

/*
 *  Atpdec 
 *  Copyright (c) 2004 by Thierry Leconte (F4DWV)
 *
 *      $Id$
 *
 *   This library is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU Library General Public License as
 *   published by the Free Software Foundation; either version 2 of
 *   the License, or (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU Library General Public License for more details.
 *
 *   You should have received a copy of the GNU Library General Public
 *   License along with this library; if not, write to the Free Software
 *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */
#include <string.h>
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846	/* for OS that don't know it */
#endif				/*  */
#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 Fi (Fp*RSMULT)
static double FreqOsc = Fc / Fe;
static double FreqLine = 1.0;

extern int getsample(float *inbuff, int nb);

static float pll(float In)
{

/* 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;
    double DF;

/* quadrature oscillator */
    Io = cos(PhaseOsc);
    Qo = sin(PhaseOsc);

/* phase detector */
    Ip = iir(In * Io, &Ifilterbuff, &PhaseFilterCf);
    Qp = iir(In * Qo, &Qfilterbuff, &PhaseFilterCf);
    DPhi = -atan2(Qp, Ip) / M_PI;

/*  loop filter  */
    DF = K1 * DPhi + FreqOsc;
    FreqOsc += K2 * DPhi;
    if (FreqOsc > ((Fc + DFc) / Fe))
	FreqOsc = (Fc + DFc) / Fe;
    if (FreqOsc < ((Fc - DFc) / Fe))
	FreqOsc = (Fc - DFc) / Fe;
    PhaseOsc += 2.0 * M_PI * DF;
    if (PhaseOsc > M_PI)
	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;

int getamp(float *ambuff, int nb)
{

#define BLKIN 1024
    float inbuff[BLKIN];
    int n;
    int res;

    res = getsample(inbuff, nb > BLKIN ? BLKIN : nb);
    for (n = 0; n < res; n++) {
	ambuff[n] = pll(inbuff[n]);
	fr = 0.25 * FreqOsc + 0.75 * fr;
    }
    return (res);
}

int getpixelv(float *pvbuff, int nb)
{

#define BLKAMP 256
    static float ambuff[BLKAMP];
    static int nam = 0;
    static int idxam = 0;
    int n;

    for (n = 0; n < nb; n++) {
	double mult;
	int shift;

	if (nam < BLKAMP) {
	    int res;
	    memmove(ambuff, &(ambuff[idxam]), nam * sizeof(float));
	    idxam = 0;
	    res = getamp(&(ambuff[nam]), BLKAMP - nam);
	    nam += res;
	    if (nam < BLKAMP)
		return (n);
	}

	mult = (double) Fi *fr / Fc * FreqLine;

	pvbuff[n] =
	    rsfir(&(ambuff[idxam]), rsfilter, RSFilterLen, offset,
		  mult) * mult * 2 * 256.0;

	shift = (int) ((RSMULT - offset + mult - 1) / mult);
	offset = shift * mult + offset - RSMULT;
	idxam += shift;
	nam -= shift;
    }
    return (nb);
}

int getpixelrow(float *pixelv)
{
    static float pixels[PixelLine + SyncFilterLen];
    static int npv = 0;
    static int synced = 0;
    static double max = 0.0;

    double corr, ecorr, lcorr;
    int res;

    if (npv > 0)
	memmove(pixelv, pixels, npv * sizeof(float));
    if (npv < SyncFilterLen + 2) {
	res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv);
	npv += res;
	if (npv < SyncFilterLen + 2)
	    return (0);
    }

    /* test sync */
    corr = fir(&(pixelv[1]), Sync, SyncFilterLen);
    ecorr = fir(pixelv, Sync, SyncFilterLen);
    lcorr = fir(&(pixelv[2]), Sync, SyncFilterLen);
    FreqLine = 1.0 + (ecorr - lcorr) / corr / PixelLine / 4.0;
    if (corr < 0.75 * max) {
	synced = 0;
	FreqLine = 1.0;
    }
    max = corr;
    if (synced < 8) {
	int shift, mshift;

	if (npv < PixelLine + SyncFilterLen) {
	    res =
		getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv);
	    npv += res;
	    if (npv < PixelLine + SyncFilterLen)
		return (0);
	}

	/* lookup sync start */
	mshift = 0;
	for (shift = 1; shift < PixelLine; shift++) {
	    double corr;

	    corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen);
	    if (corr > max) {
		mshift = shift;
		max = corr;
	    }
	}
	if (mshift != 0) {
	    memmove(pixelv, &(pixelv[mshift]),
		    (npv - mshift) * sizeof(float));
	    npv -= mshift;
	    synced = 0;
	    FreqLine = 1.0;
	} else
	    synced += 1;
    }
    if (npv < PixelLine) {
	res = getpixelv(&(pixelv[npv]), PixelLine - npv);
	npv += res;
	if (npv < PixelLine)
	    return (0);
    }
    if (npv == PixelLine) {
	npv = 0;
    } else {
	memmove(pixels, &(pixelv[PixelLine]),
		(npv - PixelLine) * sizeof(float));
	npv -= PixelLine;
    }

    return (1);
}