aptdec/dsp.c

/*
 *  Atpdec 
 *  Copyright (c) 2003 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 <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;
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;
  double ph;
  int n, 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);
}