resample.c

基于VXWORKS H323通信技术源代码

		*obuf++ = LEFT(resample->Y[i], 16);
	}
	*osamp = Nout - resample->Oskip;
}

/*
 * Do anything required when you stop reading samples.  
 * Don't close input file! 
 */
void resample_stop(effp)
eff_t effp;
{
	resample_t resample = (resample_t) effp->priv;
	
	free(resample->Imp);
	free(resample->ImpD);
	free(resample->X);
	free(resample->Y);
}

/* From resample:filters.c */

/* Sampling rate up-conversion only subroutine;
 * Slightly faster than down-conversion;
 */
HWORD SrcUp(X, Y, Factor, Time, Nx, Nwing, LpScl, Imp, ImpD, Interp)
HWORD X[], Y[];
double Factor;
UWORD *Time;
UHWORD Nx, Nwing, LpScl;
HWORD Imp[], ImpD[];
BOOL Interp;
{
   HWORD *Xp, *Ystart;
   IWORD v;

   double dt;                  /* Step through input signal */ 
   UWORD dtb;                  /* Fixed-point version of Dt */
   UWORD endTime;              /* When Time reaches EndTime, return to user */

   dt = 1.0/Factor;            /* Output sampling period */
   dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */

   Ystart = Y;
   endTime = *Time + (1<<Np)*(IWORD)Nx;
   while (*Time < endTime)
      {
      Xp = &X[*Time>>Np];      /* Ptr to current input sample */
      v = FilterUp(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),
         -1);                  /* Perform left-wing inner product */
      v += FilterUp(Imp, ImpD, Nwing, Interp, Xp+1, (HWORD)((-*Time)&Pmask),
         1);                   /* Perform right-wing inner product */
      v >>= Nhg;               /* Make guard bits */
      v *= LpScl;              /* Normalize for unity filter gain */
      *Y++ = v>>NLpScl;        /* Deposit output */
      *Time += dtb;            /* Move to next sample by time increment */
      }
   return (Y - Ystart);        /* Return the number of output samples */
}


/* Sampling rate conversion subroutine */

HWORD SrcUD(X, Y, Factor, Time, Nx, Nwing, LpScl, Imp, ImpD, Interp)
HWORD X[], Y[];
double Factor;
UWORD *Time;
UHWORD Nx, Nwing, LpScl;
HWORD Imp[], ImpD[];
BOOL Interp;
{
   HWORD *Xp, *Ystart;
   IWORD v;

   double dh;                  /* Step through filter impulse response */
   double dt;                  /* Step through input signal */
   UWORD endTime;              /* When Time reaches EndTime, return to user */
   UWORD dhb, dtb;             /* Fixed-point versions of Dh,Dt */

   dt = 1.0/Factor;            /* Output sampling period */
   dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */

   dh = MIN(Npc, Factor*Npc);  /* Filter sampling period */
   dhb = dh*(1<<Na) + 0.5;     /* Fixed-point representation */

   Ystart = Y;
   endTime = *Time + (1<<Np)*(IWORD)Nx;
   while (*Time < endTime)
      {
      Xp = &X[*Time>>Np];      /* Ptr to current input sample */
      v = FilterUD(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),
          -1, dhb);            /* Perform left-wing inner product */
      v += FilterUD(Imp, ImpD, Nwing, Interp, Xp+1, (HWORD)((-*Time)&Pmask),
           1, dhb);            /* Perform right-wing inner product */
      v >>= Nhg;               /* Make guard bits */
      v *= LpScl;              /* Normalize for unity filter gain */
      *Y++ = v>>NLpScl;        /* Deposit output */
      *Time += dtb;            /* Move to next sample by time increment */
      }
   return (Y - Ystart);        /* Return the number of output samples */
}

void LpFilter();

int makeFilter(Imp, ImpD, LpScl, Nwing, Froll, Beta)
HWORD Imp[], ImpD[];
UHWORD *LpScl, Nwing;
double Froll, Beta;
{
   double DCgain, Scl, Maxh;
   double *ImpR;
   HWORD Dh;
   LONG i, temp;

   if (Nwing > MAXNWING)                      /* Check for valid parameters */
      return(1);
   if ((Froll<=0) || (Froll>1))
      return(2);
   if (Beta < 1)
      return(3);

   ImpR = (double *) malloc(sizeof(double) * MAXNWING);
   LpFilter(ImpR, (int)Nwing, Froll, Beta, Npc); /* Design a Kaiser-window */
                                                 /* Sinc low-pass filter */

   /* Compute the DC gain of the lowpass filter, and its maximum coefficient
    * magnitude. Scale the coefficients so that the maximum coeffiecient just
    * fits in Nh-bit fixed-point, and compute LpScl as the NLpScl-bit (signed)
    * scale factor which when multiplied by the output of the lowpass filter
    * gives unity gain. */
   DCgain = 0;
   Dh = Npc;                       /* Filter sampling period for factors>=1 */
   for (i=Dh; i<Nwing; i+=Dh)
      DCgain += ImpR[i];
   DCgain = 2*DCgain + ImpR[0];    /* DC gain of real coefficients */

   for (Maxh=i=0; i<Nwing; i++)
      Maxh = MAX(Maxh, fabs(ImpR[i]));

   Scl = ((1<<(Nh-1))-1)/Maxh;     /* Map largest coeff to 16-bit maximum */
   temp = fabs((1<<(NLpScl+Nh))/(DCgain*Scl));
   if (temp >= (1L<<16)) {
      free(ImpR);
      return(4);                   /* Filter scale factor overflows UHWORD */
    }
   *LpScl = temp;

   /* Scale filter coefficients for Nh bits and convert to integer */
   if (ImpR[0] < 0)                /* Need pos 1st value for LpScl storage */
      Scl = -Scl;
   for (i=0; i<Nwing; i++)         /* Scale them */
      ImpR[i] *= Scl;
   for (i=0; i<Nwing; i++)         /* Round them */
      Imp[i] = ImpR[i] + 0.5;

   /* ImpD makes linear interpolation of the filter coefficients faster */
   for (i=0; i<Nwing-1; i++)
      ImpD[i] = Imp[i+1] - Imp[i];
   ImpD[Nwing-1] = - Imp[Nwing-1];      /* Last coeff. not interpolated */

   free(ImpR);
   return(0);
}



/* LpFilter()
 *
 * reference: "Digital Filters, 2nd edition"
 *            R.W. Hamming, pp. 178-179
 *
 * Izero() computes the 0th order modified bessel function of the first kind.
 *    (Needed to compute Kaiser window).
 *
 * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with
 *    the following characteristics:
 *
 *       c[]  = array in which to store computed coeffs
 *       frq  = roll-off frequency of filter
 *       N    = Half the window length in number of coeffs
 *       Beta = parameter of Kaiser window
 *       Num  = number of coeffs before 1/frq
 *
 * Beta trades the rejection of the lowpass filter against the transition
 *    width from passband to stopband.  Larger Beta means a slower
 *    transition and greater stopband rejection.  See Rabiner and Gold
 *    (Theory and Application of DSP) under Kaiser windows for more about
 *    Beta.  The following table from Rabiner and Gold gives some feel
 *    for the effect of Beta:
 *
 * All ripples in dB, width of transition band = D*N where N = window length
 *
 *               BETA    D       PB RIP   SB RIP
 *               2.120   1.50  +-0.27      -30
 *               3.384   2.23    0.0864    -40
 *               4.538   2.93    0.0274    -50
 *               5.658   3.62    0.00868   -60
 *               6.764   4.32    0.00275   -70
 *               7.865   5.0     0.000868  -80
 *               8.960   5.7     0.000275  -90
 *               10.056  6.4     0.000087  -100
 */


#define IzeroEPSILON 1E-21               /* Max error acceptable in Izero */

double Izero(x)
double x;
{
   double sum, u, halfx, temp;
   LONG n;

   sum = u = n = 1;
   halfx = x/2.0;
   do {
      temp = halfx/(double)n;
      n += 1;
      temp *= temp;
      u *= temp;
      sum += u;
      } while (u >= IzeroEPSILON*sum);
   return(sum);
}


void LpFilter(c,N,frq,Beta,Num)
double c[], frq, Beta;
int N, Num;
{
   double IBeta, temp;
   int i;

   /* Calculate filter coeffs: */
   c[0] = frq;
   for (i=1; i<N; i++)
      {
      temp = PI*(double)i/(double)Num;
      c[i] = sin(temp*frq)/temp;
      }

   /* Calculate and Apply Kaiser window to filter coeffs: */
   IBeta = 1.0/Izero(Beta);
   for (i=1; i<N; i++)
      {
      temp = (double)i / ((double)N * (double)1.0);
      c[i] *= Izero(Beta*sqrt(1.0-temp*temp)) * IBeta;
      }
}




IWORD FilterUp(Imp, ImpD, Nwing, Interp, Xp, Ph, Inc)
HWORD Imp[], ImpD[];
UHWORD Nwing;
BOOL Interp;
HWORD *Xp, Ph, Inc;
{
   HWORD a=0, *Hp, *Hdp=0, *End;
   IWORD v, t;

   v=0;
   Hp = &Imp[Ph>>Na];
   End = &Imp[Nwing];
   if (Interp)
      {
      Hdp = &ImpD[Ph>>Na];
      a = Ph & Amask;
      }
   /* Possible Bug: Hdp and a are not initialized if Interp == 0 */
   if (Inc == 1)                     /* If doing right wing...              */
      {                              /* ...drop extra coeff, so when Ph is  */
      End--;                         /*    0.5, we don't do too many mult's */
      if (Ph == 0)                   /* If the phase is zero...           */
         {                           /* ...then we've already skipped the */
         Hp += Npc;                  /*    first sample, so we must also  */
         Hdp += Npc;                 /*    skip ahead in Imp[] and ImpD[] */
         }
      }
   while (Hp < End)
      {
      t = *Hp;                       /* Get filter coeff */
      if (Interp)
         {
         t += (((IWORD)*Hdp)*a)>>Na;  /* t is now interp'd filter coeff */
         Hdp += Npc;                 /* Filter coeff differences step */
	 }
      t *= *Xp;      /* Mult coeff by input sample */
	  if (t & (1<<(Nhxn-1)))  /* Round, if needed */
		 t += (1<<(Nhxn-1));
      t >>= Nhxn;    /* Leave some guard bits, but come back some */
      v += t;        /* The filter output */
      Hp += Npc;     /* Filter coeff step */
      Xp += Inc;     /* Input signal step. NO CHECK ON ARRAY BOUNDS */
      }
   return(v);
}


IWORD FilterUD(Imp, ImpD, Nwing, Interp, Xp, Ph, Inc, dhb)
HWORD Imp[], ImpD[];
UHWORD Nwing;
BOOL Interp;
HWORD *Xp, Ph, Inc;
UHWORD dhb;
{
   HWORD a, *Hp, *Hdp, *End;
   IWORD v, t;
   UWORD Ho;

   v=0;
   Ho = (Ph*(UWORD)dhb)>>Np;
   End = &Imp[Nwing];
   if (Inc == 1)                     /* If doing right wing...              */
      {                              /* ...drop extra coeff, so when Ph is  */
      End--;                         /*    0.5, we don't do too many mult's */
      if (Ph == 0)                   /* If the phase is zero...           */
         Ho += dhb;                  /* ...then we've already skipped the */
      }                              /*    first sample, so we must also  */
                                     /*    skip ahead in Imp[] and ImpD[] */
   while ((Hp = &Imp[Ho>>Na]) < End)
      {
      t = *Hp;       /* Get IR sample */
      if (Interp)
         {
         Hdp = &ImpD[Ho>>Na]; /* get interp (lower Na) bits from diff table */
         a = Ho & Amask;                  /* a is logically between 0 and 1 */
         t += (((IWORD)*Hdp)*a)>>Na;      /* t is now interp'd filter coeff */
	 }
      t *= *Xp;      /* Mult coeff by input sample */
	  if (t & (1<<(Nhxn-1)))  /* Round, if needed */
		 t += (1<<(Nhxn-1));
      t >>= Nhxn;    /* Leave some guard bits, but come back some */
      v += t;        /* The filter output */
      Ho += dhb;     /* IR step */
      Xp += Inc;     /* Input signal step. NO CHECK ON ARRAY BOUNDS */
      }
   return(v);
}