Пример #1
1
word32_t ChebyshevPolynomial(word16_t x, word32_t f[])
{
    /* bk in Q15*/
    word32_t bk;
    word32_t bk1 = ADD32(SHL(x,1), f[1]); /* init: b4=2x+f1 */
    word32_t bk2 = ONE_IN_Q15; /* init: b5=1 */
    uint8_t k;
    for (k=3; k>0; k--) { /* at the end of loop execution we have b1 in bk1 and b2 in bk2 */
        bk = SUB32(ADD32(SHL(MULT16_32_Q15(x,bk1), 1), f[5-k]), bk2); /* bk = 2*x*bk1 − bk2 + f(5-k) all in Q15*/
        bk2 = bk1;
        bk1 = bk;
    }

    return SUB32(ADD32(MULT16_32_Q15(x,bk1), SHR(f[5],1)), bk2); /* C(x) = x*b1 - b2 + f(5)/2 */
}
Пример #2
0
/* Computes a rough approximation of log2(2^a + 2^b) */
static opus_val16 logSum(opus_val16 a, opus_val16 b)
{
   opus_val16 max;
   opus_val32 diff;
   opus_val16 frac;
   static const opus_val16 diff_table[17] = {
         QCONST16(0.5000000f, DB_SHIFT), QCONST16(0.2924813f, DB_SHIFT), QCONST16(0.1609640f, DB_SHIFT), QCONST16(0.0849625f, DB_SHIFT),
         QCONST16(0.0437314f, DB_SHIFT), QCONST16(0.0221971f, DB_SHIFT), QCONST16(0.0111839f, DB_SHIFT), QCONST16(0.0056136f, DB_SHIFT),
         QCONST16(0.0028123f, DB_SHIFT)
   };
   int low;
   if (a>b)
   {
      max = a;
      diff = SUB32(EXTEND32(a),EXTEND32(b));
   } else {
      max = b;
      diff = SUB32(EXTEND32(b),EXTEND32(a));
   }
   if (!(diff < QCONST16(8.f, DB_SHIFT)))  /* inverted to catch NaNs */
      return max;
#ifdef FIXED_POINT
   low = SHR32(diff, DB_SHIFT-1);
   frac = SHL16(diff - SHL16(low, DB_SHIFT-1), 16-DB_SHIFT);
#else
   low = (int)floor(2*diff);
   frac = 2*diff - low;
#endif
   return max + diff_table[low] + MULT16_16_Q15(frac, SUB16(diff_table[low+1], diff_table[low]));
}
Пример #3
0
static void kf_bfly2(
        kiss_fft_cpx * Fout,
        const size_t fstride,
        const kiss_fft_cfg st,
        int m,
        int N,
        int mm
        )
{
    kiss_fft_cpx * Fout2;
    kiss_fft_cpx * tw1;
    kiss_fft_cpx t;
    if (!st->inverse) {
       int i,j;
       kiss_fft_cpx * Fout_beg = Fout;
       for (i=0;i<N;i++)
       {
          Fout = Fout_beg + i*mm;
          Fout2 = Fout + m;
          tw1 = st->twiddles;
          for(j=0;j<m;j++)
          {
             /* Almost the same as the code path below, except that we divide the input by two
              (while keeping the best accuracy possible) */
             ms_word32_t tr, ti;
             tr = SHR32(SUB32(MULT16_16(Fout2->r , tw1->r),MULT16_16(Fout2->i , tw1->i)), 1);
             ti = SHR32(ADD32(MULT16_16(Fout2->i , tw1->r),MULT16_16(Fout2->r , tw1->i)), 1);
             tw1 += fstride;
             Fout2->r = PSHR32(SUB32(SHL32(EXTEND32(Fout->r), 14), tr), 15);
             Fout2->i = PSHR32(SUB32(SHL32(EXTEND32(Fout->i), 14), ti), 15);
             Fout->r = PSHR32(ADD32(SHL32(EXTEND32(Fout->r), 14), tr), 15);
             Fout->i = PSHR32(ADD32(SHL32(EXTEND32(Fout->i), 14), ti), 15);
             ++Fout2;
             ++Fout;
          }
       }
    } else {
       int i,j;
       kiss_fft_cpx * Fout_beg = Fout;
       for (i=0;i<N;i++)
       {
          Fout = Fout_beg + i*mm;
          Fout2 = Fout + m;
          tw1 = st->twiddles;
          for(j=0;j<m;j++)
          {
             C_MUL (t,  *Fout2 , *tw1);
             tw1 += fstride;
             C_SUB( *Fout2 ,  *Fout , t );
             C_ADDTO( *Fout ,  t );
             ++Fout2;
             ++Fout;
          }
       }
    }
}
Пример #4
0
static inline void spectral_mul_accum16(const spx_word16_t *X, const spx_word16_t *Y, spx_word16_t *acc, int N, int M)
{
   int i,j;
   spx_word32_t tmp1=0,tmp2=0;
   for (j=0;j<M;j++)
   {
      tmp1 = MAC16_16(tmp1, X[j*N],Y[j*N]);
   }
   acc[0] = PSHR32(tmp1,WEIGHT_SHIFT);
   for (i=1;i<N-1;i+=2)
   {
      tmp1 = tmp2 = 0;
      for (j=0;j<M;j++)
      {
         tmp1 = SUB32(MAC16_16(tmp1, X[j*N+i],Y[j*N+i]), MULT16_16(X[j*N+i+1],Y[j*N+i+1]));
         tmp2 = MAC16_16(MAC16_16(tmp2, X[j*N+i+1],Y[j*N+i]), X[j*N+i], Y[j*N+i+1]);
      }
      acc[i] = PSHR32(tmp1,WEIGHT_SHIFT);
      acc[i+1] = PSHR32(tmp2,WEIGHT_SHIFT);
   }
   tmp1 = tmp2 = 0;
   for (j=0;j<M;j++)
   {
      tmp1 = MAC16_16(tmp1, X[(j+1)*N-1],Y[(j+1)*N-1]);
   }
   acc[N-1] = PSHR32(tmp1,WEIGHT_SHIFT);
}
Пример #5
0
/*Finds the indices of the n-best entries in a codebook*/
void vq_nbest(spx_word16_t *in, const spx_word16_t *codebook, int len, int entries, spx_word32_t *E, int N, int *nbest, spx_word32_t *best_dist, char *stack)
{
   int i,j,k,used;
   used = 0;
   for (i=0;i<entries;i++)
   {
      spx_word32_t dist=0;
      for (j=0;j<len;j++)
         dist = MAC16_16(dist,in[j],*codebook++);
#ifdef FIXED_POINT
      dist=SUB32(SHR32(E[i],1),dist);
#else
      dist=.5f*E[i]-dist;
#endif
      if (i<N || dist<best_dist[N-1])
      {
         for (k=N-1; (k >= 1) && (k > used || dist < best_dist[k-1]); k--)
         {
            best_dist[k]=best_dist[k-1];
            nbest[k] = nbest[k-1];
         }
         best_dist[k]=dist;
         nbest[k]=i;
         used++;
      }
   }
}
Пример #6
0
spx_word32_t speex_rand(spx_word16_t std, spx_int32_t *seed)
{
   spx_word32_t res;
   *seed = 1664525 * *seed + 1013904223;
   res = MULT16_16(EXTRACT16(SHR32(*seed,16)),std);
   return SUB32(res, SHR(res, 3));
}
Пример #7
0
void REM32(reg_t r1, reg_t r2, reg_t r3, reg_t scratch)
{
  MOV32(scratch, r2); 
  MOVIM32(r1, 0);

  EQ32(scratch+4, r3, R_ZERO);
  if (REG32(scratch+4))
  {
    goto _end;
  }

  GTE32(scratch+4, scratch, r3, scratch+16);
  GT32(scratch+8, scratch, R_ZERO);
  LAND32(scratch+12, scratch+4, scratch+8);
  while (REG32(scratch+12))
  {
    SUB32(scratch, scratch, r3);

    GTE32(scratch+4, scratch, r3, scratch+16);
    GT32(scratch+8, scratch, R_ZERO);
    LAND32(scratch+12, scratch+4, scratch+8);
  } 

  MOV32(r1, scratch);
_end:
  return;
}
Пример #8
0
static opus_val32 silk_resampler_down2_hp(
    opus_val32                  *S,                 /* I/O  State vector [ 2 ]                                          */
    opus_val32                  *out,               /* O    Output signal [ floor(len/2) ]                              */
    const opus_val32            *in,                /* I    Input signal [ len ]                                        */
    int                         inLen               /* I    Number of input samples                                     */
)
{
    int k, len2 = inLen/2;
    opus_val32 in32, out32, out32_hp, Y, X;
    opus_val64 hp_ener = 0;
    /* Internal variables and state are in Q10 format */
    for( k = 0; k < len2; k++ ) {
        /* Convert to Q10 */
        in32 = in[ 2 * k ];

        /* All-pass section for even input sample */
        Y      = SUB32( in32, S[ 0 ] );
        X      = MULT16_32_Q15(QCONST16(0.6074371f, 15), Y);
        out32  = ADD32( S[ 0 ], X );
        S[ 0 ] = ADD32( in32, X );
        out32_hp = out32;
        /* Convert to Q10 */
        in32 = in[ 2 * k + 1 ];

        /* All-pass section for odd input sample, and add to output of previous section */
        Y      = SUB32( in32, S[ 1 ] );
        X      = MULT16_32_Q15(QCONST16(0.15063f, 15), Y);
        out32  = ADD32( out32, S[ 1 ] );
        out32  = ADD32( out32, X );
        S[ 1 ] = ADD32( in32, X );

        Y      = SUB32( -in32, S[ 2 ] );
        X      = MULT16_32_Q15(QCONST16(0.15063f, 15), Y);
        out32_hp  = ADD32( out32_hp, S[ 2 ] );
        out32_hp  = ADD32( out32_hp, X );
        S[ 2 ] = ADD32( -in32, X );

        hp_ener += out32_hp*(opus_val64)out32_hp;
        /* Add, convert back to int16 and store to output */
        out[ k ] = HALF32(out32);
    }
#ifdef FIXED_POINT
    /* len2 can be up to 480, so we shift by 8 more to make it fit. */
    hp_ener = hp_ener >> (2*SIG_SHIFT + 8);
#endif
    return (opus_val32)hp_ener;
}
Пример #9
0
/** Forced pitch delay and gain */
int forced_pitch_quant(spx_word16_t target[],	/* Target vector */
		       spx_word16_t * sw, spx_coef_t ak[],	/* LPCs for this subframe */
		       spx_coef_t awk1[],	/* Weighted LPCs #1 for this subframe */
		       spx_coef_t awk2[],	/* Weighted LPCs #2 for this subframe */
		       spx_sig_t exc[],	/* Excitation */
		       const void *par, int start,	/* Smallest pitch value allowed */
		       int end,	/* Largest pitch value allowed */
		       spx_word16_t pitch_coef,	/* Voicing (pitch) coefficient */
		       int p,	/* Number of LPC coeffs */
		       int nsf,	/* Number of samples in subframe */
		       SpeexBits * bits,
		       char *stack,
		       spx_word16_t * exc2,
		       spx_word16_t * r,
		       int complexity,
		       int cdbk_offset,
		       int plc_tuning, spx_word32_t * cumul_gain)
{
	(void)sw;
	(void)par;
	(void)end;
	(void)bits;
	(void)r;
	(void)complexity;
	(void)cdbk_offset;
	(void)plc_tuning;
	(void)cumul_gain;
	int i;
	spx_word16_t res[nsf];
#ifdef FIXED_POINT
	if (pitch_coef > 63)
		pitch_coef = 63;
#else
	if (pitch_coef > .99)
		pitch_coef = .99;
#endif
	for (i = 0; i < nsf && i < start; i++) {
		exc[i] = MULT16_16(SHL16(pitch_coef, 7), exc2[i - start]);
	}
	for (; i < nsf; i++) {
		exc[i] = MULT16_32_Q15(SHL16(pitch_coef, 9), exc[i - start]);
	}
	for (i = 0; i < nsf; i++)
		res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT - 1));
	syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack);
	for (i = 0; i < nsf; i++)
		target[i] =
		    EXTRACT16(SATURATE
			      (SUB32(EXTEND32(target[i]), EXTEND32(res[i])),
			       32700));
	return start;
}
Пример #10
0
static spx_word16_t spx_cos(spx_word16_t x)
{
   spx_word16_t x2;

   if (x<12868)
   {
      x2 = MULT16_16_P13(x,x);
      return ADD32(C1, MULT16_16_P13(x2, ADD32(C2, MULT16_16_P13(x2, ADD32(C3, MULT16_16_P13(C4, x2))))));
   } else {
      x = 25736-x;
      x2 = MULT16_16_P13(x,x);
      return SUB32(-C1, MULT16_16_P13(x2, ADD32(C2, MULT16_16_P13(x2, ADD32(C3, MULT16_16_P13(C4, x2))))));
      /*return SUB32(-C1, MULT16_16_Q13(x2, ADD32(C2, MULT16_16_Q13(C3, x2))));*/
   }
}
Пример #11
0
static void cubic_coef(spx_word16_t x, spx_word16_t interp[4])
{
   /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
   but I know it's MMSE-optimal on a sinc */
   spx_word16_t x2, x3;
   x2 = MULT16_16_P15(x, x);
   x3 = MULT16_16_P15(x, x2);
   interp[0] = PSHR32(MULT16_16(QCONST16(-0.16667f, 15),x) + MULT16_16(QCONST16(0.16667f, 15),x3),15);
   interp[1] = EXTRACT16(EXTEND32(x) + SHR32(SUB32(EXTEND32(x2),EXTEND32(x3)),1));
   interp[3] = PSHR32(MULT16_16(QCONST16(-0.33333f, 15),x) + MULT16_16(QCONST16(.5f,15),x2) - MULT16_16(QCONST16(0.16667f, 15),x3),15);
   /* Just to make sure we don't have rounding problems */
   interp[2] = Q15_ONE-interp[0]-interp[1]-interp[3];
   if (interp[2]<32767)
      interp[2]+=1;
}
Пример #12
0
static inline spx_word32_t compute_pitch_error(spx_word16_t* C, spx_word16_t* g, spx_word16_t pitch_control) {
    spx_word32_t sum = 0;
    sum = ADD32(sum, MULT16_16(MULT16_16_16(g[0], pitch_control), C[0]));
    sum = ADD32(sum, MULT16_16(MULT16_16_16(g[1], pitch_control), C[1]));
    sum = ADD32(sum, MULT16_16(MULT16_16_16(g[2], pitch_control), C[2]));
    sum = SUB32(sum, MULT16_16(MULT16_16_16(g[0], g[1]), C[3]));
    sum = SUB32(sum, MULT16_16(MULT16_16_16(g[2], g[1]), C[4]));
    sum = SUB32(sum, MULT16_16(MULT16_16_16(g[2], g[0]), C[5]));
    sum = SUB32(sum, MULT16_16(MULT16_16_16(g[0], g[0]), C[6]));
    sum = SUB32(sum, MULT16_16(MULT16_16_16(g[1], g[1]), C[7]));
    sum = SUB32(sum, MULT16_16(MULT16_16_16(g[2], g[2]), C[8]));
    return sum;
}
Пример #13
0
/* returns minimum mean square error    */
spx_word32_t _spx_lpc(
spx_coef_t       *lpc, /* out: [0...p-1] LPC coefficients      */
const spx_word16_t *ac,  /* in:  [0...p] autocorrelation values  */
int          p
)
{
   int i, j;  
   spx_word16_t r;
   spx_word16_t error = ac[0];

   if (ac[0] == 0)
   {
      for (i = 0; i < p; i++)
         lpc[i] = 0;
      return 0;
   }

   for (i = 0; i < p; i++) {

      /* Sum up this iteration's reflection coefficient */
      spx_word32_t rr = NEG32(SHL32(EXTEND32(ac[i + 1]),13));
      for (j = 0; j < i; j++) 
         rr = SUB32(rr,MULT16_16(lpc[j],ac[i - j]));
#ifdef FIXED_POINT
      r = DIV32_16(rr+PSHR32(error,1),ADD16(error,8));
#else
      r = rr/(error+.003*ac[0]);
#endif
      /*  Update LPC coefficients and total error */
      lpc[i] = r;
      for (j = 0; j < i>>1; j++) 
      {
         spx_word16_t tmp  = lpc[j];
         lpc[j]     = MAC16_16_P13(lpc[j],r,lpc[i-1-j]);
         lpc[i-1-j] = MAC16_16_P13(lpc[i-1-j],r,tmp);
      }
      if (i & 1) 
         lpc[j] = MAC16_16_P13(lpc[j],lpc[j],r);

      error = SUB16(error,MULT16_16_Q13(r,MULT16_16_Q13(error,r)));
   }
   return error;
}
Пример #14
0
void filter_mem2(const spx_sig_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
{
   int i,j;
   spx_sig_t xi,yi,nyi;

   for (i=0;i<N;i++)
   {
      int xh,xl,yh,yl;
      xi=SATURATE(x[i],805306368);
      yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
      nyi = -yi;
      xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
      for (j=0;j<ord-1;j++)
      {
         mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
      }
      mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
      y[i] = yi;
   }
}
Пример #15
0
void bcg729Encoder(bcg729EncoderChannelContextStruct *encoderChannelContext, int16_t inputFrame[], uint8_t bitStream[])
{
	int i;
	uint16_t parameters[NB_PARAMETERS]; /* the output parameters in an array */

	/* internal buffers which we do not need to keep between calls */
	word16_t LPCoefficients[NB_LSP_COEFF]; /* the LP coefficients in Q3.12 */
	word16_t qLPCoefficients[2*NB_LSP_COEFF]; /* the quantized LP coefficients in Q3.12 computed from the qLSP one after interpolation: two sets, one for each subframe */
	word16_t weightedqLPCoefficients[2*NB_LSP_COEFF]; /* the qLP coefficients in Q3.12 weighted according to spec A3.3.3 */
	word16_t LSPCoefficients[NB_LSP_COEFF]; /* the LSP coefficients in Q15 */
	word16_t qLSPCoefficients[NB_LSP_COEFF]; /* the quantized LSP coefficients in Q15 */
	word16_t interpolatedqLSP[NB_LSP_COEFF]; /* the interpolated qLSP used for first subframe in Q15 */


	/*****************************************************************************************/
	/*** on frame basis : preProcessing, LP Analysis, Open-loop pitch search               ***/
	preProcessing(encoderChannelContext, inputFrame, encoderChannelContext->signalLastInputFrame); /* output of the function in the signal buffer */

	computeLP(encoderChannelContext->signalBuffer, LPCoefficients); /* use the whole signal Buffer for windowing and autocorrelation */
	/*** compute LSP: it might fail, get the previous one in this case ***/
	if (!LP2LSPConversion(LPCoefficients, LSPCoefficients)) {
		/* unable to find the 10 roots repeat previous LSP */
		memcpy(LSPCoefficients, encoderChannelContext->previousLSPCoefficients, NB_LSP_COEFF*sizeof(word16_t));
	}

	/*** LSPQuantization and compute L0, L1, L2, L3: the first four parameters ***/
	LSPQuantization(encoderChannelContext, LSPCoefficients, qLSPCoefficients, parameters);
	
	/*** interpolate qLSP and convert to LP ***/
	interpolateqLSP(encoderChannelContext->previousqLSPCoefficients, qLSPCoefficients, interpolatedqLSP);
	/* copy the currentqLSP to previousqLSP buffer */
	for (i=0; i<NB_LSP_COEFF; i++) {
		encoderChannelContext->previousqLSPCoefficients[i] = qLSPCoefficients[i];
	}

	/* first subframe */
	qLSP2LP(interpolatedqLSP, qLPCoefficients);
	/* second subframe */
	qLSP2LP(qLSPCoefficients, &(qLPCoefficients[NB_LSP_COEFF]));

	/*** Compute the weighted Quantized LP Coefficients according to spec A3.3.3 ***/
	/*  weightedqLPCoefficients[0] = qLPCoefficients[0]*Gamma^(i+1) (i=0..9) with Gamma = 0.75 in Q15 */
	weightedqLPCoefficients[0] = MULT16_16_P15(qLPCoefficients[0], GAMMA_E1);
	weightedqLPCoefficients[1] = MULT16_16_P15(qLPCoefficients[1], GAMMA_E2);
	weightedqLPCoefficients[2] = MULT16_16_P15(qLPCoefficients[2], GAMMA_E3);
	weightedqLPCoefficients[3] = MULT16_16_P15(qLPCoefficients[3], GAMMA_E4);
	weightedqLPCoefficients[4] = MULT16_16_P15(qLPCoefficients[4], GAMMA_E5);
	weightedqLPCoefficients[5] = MULT16_16_P15(qLPCoefficients[5], GAMMA_E6);
	weightedqLPCoefficients[6] = MULT16_16_P15(qLPCoefficients[6], GAMMA_E7);
	weightedqLPCoefficients[7] = MULT16_16_P15(qLPCoefficients[7], GAMMA_E8);
	weightedqLPCoefficients[8] = MULT16_16_P15(qLPCoefficients[8], GAMMA_E9);
	weightedqLPCoefficients[9] = MULT16_16_P15(qLPCoefficients[9], GAMMA_E10);
	weightedqLPCoefficients[10] = MULT16_16_P15(qLPCoefficients[10], GAMMA_E1);
	weightedqLPCoefficients[11] = MULT16_16_P15(qLPCoefficients[11], GAMMA_E2);
	weightedqLPCoefficients[12] = MULT16_16_P15(qLPCoefficients[12], GAMMA_E3);
	weightedqLPCoefficients[13] = MULT16_16_P15(qLPCoefficients[13], GAMMA_E4);
	weightedqLPCoefficients[14] = MULT16_16_P15(qLPCoefficients[14], GAMMA_E5);
	weightedqLPCoefficients[15] = MULT16_16_P15(qLPCoefficients[15], GAMMA_E6);
	weightedqLPCoefficients[16] = MULT16_16_P15(qLPCoefficients[16], GAMMA_E7);
	weightedqLPCoefficients[17] = MULT16_16_P15(qLPCoefficients[17], GAMMA_E8);
	weightedqLPCoefficients[18] = MULT16_16_P15(qLPCoefficients[18], GAMMA_E9);
	weightedqLPCoefficients[19] = MULT16_16_P15(qLPCoefficients[19], GAMMA_E10);

	/*** Compute weighted signal according to spec A3.3.3, this function also set LPResidualSignal(entire frame values) as specified in eq A.3 in excitationVector[L_PAST_EXCITATION] ***/
	computeWeightedSpeech(encoderChannelContext->signalCurrentFrame, qLPCoefficients, weightedqLPCoefficients, &(encoderChannelContext->weightedInputSignal[MAXIMUM_INT_PITCH_DELAY]), &(encoderChannelContext->excitationVector[L_PAST_EXCITATION])); /* weightedInputSignal contains MAXIMUM_INT_PITCH_DELAY values from previous frame, points to current frame  */

	/*** find the open loop pitch delay ***/
	uint16_t openLoopPitchDelay = findOpenLoopPitchDelay(&(encoderChannelContext->weightedInputSignal[MAXIMUM_INT_PITCH_DELAY]));

	/* define boundaries for closed loop pitch delay search as specified in 3.7 */
	int16_t intPitchDelayMin = openLoopPitchDelay-3;
	if (intPitchDelayMin < 20) {
		intPitchDelayMin = 20;
	}
	int16_t intPitchDelayMax = intPitchDelayMin + 6;
	if (intPitchDelayMax > MAXIMUM_INT_PITCH_DELAY) {
		intPitchDelayMax = MAXIMUM_INT_PITCH_DELAY;
		intPitchDelayMin = MAXIMUM_INT_PITCH_DELAY - 6;
	}

	/*****************************************************************************************/
	/* loop over the two subframes: Closed-loop pitch search(adaptative codebook), fixed codebook, memory update */
	/* set index and buffers */
	int subframeIndex;
	int LPCoefficientsIndex = 0;
	int parametersIndex = 4; /* index to insert parameters in the parameters output array */
	word16_t impulseResponseInput[L_SUBFRAME]; /* input buffer for the impulse response computation: in Q12, 1 followed by all zeros see spec A3.5*/
	impulseResponseInput[0] = ONE_IN_Q12;
	memset(&(impulseResponseInput[1]), 0, (L_SUBFRAME-1)*sizeof(word16_t));

	for (subframeIndex=0; subframeIndex<L_FRAME; subframeIndex+=L_SUBFRAME) {
		/*** Compute the impulse response : filter a subframe long buffer filled with unit and only zero through the 1/weightedqLPCoefficients as in spec A.3.5 ***/
		word16_t impulseResponseBuffer[NB_LSP_COEFF+L_SUBFRAME]; /* impulseResponseBuffer in Q12, need NB_LSP_COEFF as past value to go through filtering function */
		memset(impulseResponseBuffer, 0, (NB_LSP_COEFF)*sizeof(word16_t)); /* set the past values to zero */
		synthesisFilter(impulseResponseInput, &(weightedqLPCoefficients[LPCoefficientsIndex]), &(impulseResponseBuffer[NB_LSP_COEFF]));
	
		/*** Compute the target signal (x[n]) as in spec A.3.6 in Q0 ***/
		/* excitationVector[L_PAST_EXCITATION+subframeIndex] currently store in Q0 the LPResidualSignal as in spec A.3.3 eq A.3*/
		synthesisFilter( &(encoderChannelContext->excitationVector[L_PAST_EXCITATION+subframeIndex]), &(weightedqLPCoefficients[LPCoefficientsIndex]), &(encoderChannelContext->targetSignal[NB_LSP_COEFF]));

		/*** Adaptative Codebook search : compute the intPitchDelay, fracPitchDelay and associated parameter, compute also the adaptative codebook vector used to generate the excitation ***/
		/* after this call, the excitationVector[L_PAST_EXCITATION + subFrameIndex] contains the adaptative codebook vector as in spec 3.7.1 */
		int16_t intPitchDelay, fracPitchDelay;
		adaptativeCodebookSearch(&(encoderChannelContext->excitationVector[L_PAST_EXCITATION + subframeIndex]), &intPitchDelayMin, &intPitchDelayMax, &(impulseResponseBuffer[NB_LSP_COEFF]), &(encoderChannelContext->targetSignal[NB_LSP_COEFF]),
			&intPitchDelay, &fracPitchDelay, &(parameters[parametersIndex]), subframeIndex);

		/*** Compute adaptative codebook gain spec 3.7.3, result in Q14 ***/
		/* compute the filtered adaptative codebook vector spec 3.7.3 */
		/* this computation makes use of two partial results used for gainQuantization too (yy and xy in eq63), they are part of the function output */
		/* note spec 3.7.3 eq44 make use of convolution of impulseResponse and adaptative codebook vector to compute the filtered version */
		/* in the Annex A, the filter being simpler, it's faster to directly filter the the vector using the  weightedqLPCoefficients */
		word16_t filteredAdaptativeCodebookVector[NB_LSP_COEFF+L_SUBFRAME]; /* in Q0, the first NB_LSP_COEFF words are set to zero and used by filter only */
		memset(filteredAdaptativeCodebookVector, 0, NB_LSP_COEFF*sizeof(word16_t));
		synthesisFilter(&(encoderChannelContext->excitationVector[L_PAST_EXCITATION + subframeIndex]), &(weightedqLPCoefficients[LPCoefficientsIndex]), &(filteredAdaptativeCodebookVector[NB_LSP_COEFF]));

		word64_t gainQuantizationXy, gainQuantizationYy; /* used to store in Q0 values reused in gain quantization */

		word16_t adaptativeCodebookGain = computeAdaptativeCodebookGain(&(encoderChannelContext->targetSignal[NB_LSP_COEFF]), &(filteredAdaptativeCodebookVector[NB_LSP_COEFF]), &gainQuantizationXy, &gainQuantizationYy); /* gain in Q14 */
		
		/* increase parameters index and compute P0 if needed */
		parametersIndex++;
		if (subframeIndex==0) { /* first subframe compute P0, the parity bit of P1 */
			parameters[parametersIndex] = computeParity(parameters[parametersIndex-1]);
			parametersIndex++;
		}

		/*** Fixed Codebook Search : compute the parameters for fixed codebook and the regular and convolved version of the fixed codebook vector ***/
		word16_t fixedCodebookVector[L_SUBFRAME]; /* in Q13 */
		word16_t convolvedFixedCodebookVector[L_SUBFRAME]; /* in Q12 */
		fixedCodebookSearch(&(encoderChannelContext->targetSignal[NB_LSP_COEFF]), &(impulseResponseBuffer[NB_LSP_COEFF]), intPitchDelay, encoderChannelContext->lastQuantizedAdaptativeCodebookGain, &(filteredAdaptativeCodebookVector[NB_LSP_COEFF]), adaptativeCodebookGain,
			&(parameters[parametersIndex]), &(parameters[parametersIndex+1]), fixedCodebookVector, convolvedFixedCodebookVector);
		parametersIndex+=2;

		/*** gains Quantization ***/
		word16_t quantizedAdaptativeCodebookGain; /* in Q14 */
		word16_t quantizedFixedCodebookGain; /* in Q1 */
		gainQuantization(encoderChannelContext, &(encoderChannelContext->targetSignal[NB_LSP_COEFF]), &(filteredAdaptativeCodebookVector[NB_LSP_COEFF]), convolvedFixedCodebookVector, fixedCodebookVector, gainQuantizationXy, gainQuantizationYy,
			&quantizedAdaptativeCodebookGain, &quantizedFixedCodebookGain, &(parameters[parametersIndex]), &(parameters[parametersIndex+1]));
		parametersIndex+=2;
		
		/*** subframe basis indexes and memory updates ***/
		LPCoefficientsIndex+= NB_LSP_COEFF;
		encoderChannelContext->lastQuantizedAdaptativeCodebookGain = quantizedAdaptativeCodebookGain;
		if (encoderChannelContext->lastQuantizedAdaptativeCodebookGain>ONE_POINT_2_IN_Q14) encoderChannelContext->lastQuantizedAdaptativeCodebookGain = ONE_POINT_2_IN_Q14;
		if (encoderChannelContext->lastQuantizedAdaptativeCodebookGain<O2_IN_Q14) encoderChannelContext->lastQuantizedAdaptativeCodebookGain = O2_IN_Q14;
		/* compute excitation for current subframe as in spec A.3.10 */
		/* excitationVector[L_PAST_EXCITATION + subframeIndex] currently contains in Q0 the adaptative codebook vector, quantizedAdaptativeCodebookGain in Q14 */
		/* fixedCodebookVector in Q13, quantizedFixedCodebookGain in Q1 */
		for (i=0; i<L_SUBFRAME; i++) {
			encoderChannelContext->excitationVector[L_PAST_EXCITATION + subframeIndex + i] = (word16_t)(SATURATE(PSHR(ADD32(MULT16_16(encoderChannelContext->excitationVector[L_PAST_EXCITATION + subframeIndex + i], quantizedAdaptativeCodebookGain),
											MULT16_16(fixedCodebookVector[i], quantizedFixedCodebookGain)), 14), MAXINT16)); /* result in Q0 */
		}

		/* update targetSignal memory as in spec A.3.10 */
		quantizedAdaptativeCodebookGain = PSHR(quantizedAdaptativeCodebookGain, 1); /* quantizedAdaptativeCodebookGain in Q13 */
		for (i=0; i<NB_LSP_COEFF; i++) {
			/* targetSignal[i] = targetSignal[L_SUBFRAME+i] - quantizedAdaptativeCodebookGain*filteredAdaptativeCodebookVector[L_SUBFRAME+i] - quantizedFixedCodebookGain*convolvedFixedCodebookVector[L_SUBFRAME-NB_LSP_COEFF+i]*/
			word32_t acc = MAC16_16(MULT16_16(quantizedAdaptativeCodebookGain, filteredAdaptativeCodebookVector[L_SUBFRAME+i]), quantizedFixedCodebookGain, convolvedFixedCodebookVector[L_SUBFRAME-NB_LSP_COEFF+i]); /* acc in Q13 */
			encoderChannelContext->targetSignal[i] = (word16_t)(SATURATE(SUB32(encoderChannelContext->targetSignal[L_SUBFRAME+i], PSHR(acc, 13)), MAXINT16));
			
		}
	}

	/*****************************************************************************************/
	/*** frame basis memory updates                                                        ***/
	/* shift left by L_FRAME the signal buffer */
	memmove(encoderChannelContext->signalBuffer, &(encoderChannelContext->signalBuffer[L_FRAME]), (L_LP_ANALYSIS_WINDOW-L_FRAME)*sizeof(word16_t)); 
	/* update previousLSP coefficient buffer */
	memcpy(encoderChannelContext->previousLSPCoefficients, LSPCoefficients, NB_LSP_COEFF*sizeof(word16_t));
	memcpy(encoderChannelContext->previousqLSPCoefficients, qLSPCoefficients, NB_LSP_COEFF*sizeof(word16_t));
	/* shift left by L_FRAME the weightedInputSignal buffer */
	memmove(encoderChannelContext->weightedInputSignal, &(encoderChannelContext->weightedInputSignal[L_FRAME]), MAXIMUM_INT_PITCH_DELAY*sizeof(word16_t));
	/* shift left by L_FRAME the excitationVector */
	memmove(encoderChannelContext->excitationVector, &(encoderChannelContext->excitationVector[L_FRAME]), L_PAST_EXCITATION*sizeof(word16_t));

	/*** Convert array of parameters into bitStream ***/
	parametersArray2BitStream(parameters, bitStream);

	return;
}
Пример #16
0
Файл: ltp.c Проект: Affix/fgcom
void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
    int i,j,k;
    VARDECL(spx_word32_t *best_score);
    spx_word32_t e0;
    VARDECL(spx_word32_t *corr);
    VARDECL(spx_word32_t *energy);
    VARDECL(spx_word32_t *score);
#ifdef FIXED_POINT
    VARDECL(spx_word16_t *swn2);
#endif
    spx_word16_t *swn;

    ALLOC(best_score, N, spx_word32_t);
    ALLOC(corr, end-start+1, spx_word32_t);
    ALLOC(energy, end-start+2, spx_word32_t);
    ALLOC(score, end-start+1, spx_word32_t);

#ifdef FIXED_POINT
    ALLOC(swn2, end+len, spx_word16_t);
    normalize16(sw-end, swn2, 16384, end+len);
    swn = swn2 + end;
#else
    swn = sw;
#endif

    for (i=0; i<N; i++)
    {
        best_score[i]=-1;
        pitch[i]=start;
    }


    energy[0]=inner_prod(swn-start, swn-start, len);
    e0=inner_prod(swn, swn, len);
    for (i=start; i<=end; i++)
    {
        /* Update energy for next pitch*/
        energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(swn[-i-1],swn[-i-1]),6)), SHR32(MULT16_16(swn[-i+len-1],swn[-i+len-1]),6));
    }

    pitch_xcorr(swn, swn-end, corr, len, end-start+1, stack);

#ifdef FIXED_POINT
    {
        VARDECL(spx_word16_t *corr16);
        VARDECL(spx_word16_t *ener16);
        ALLOC(corr16, end-start+1, spx_word16_t);
        ALLOC(ener16, end-start+1, spx_word16_t);
        normalize16(corr, corr16, 16384, end-start+1);
        normalize16(energy, ener16, 16384, end-start+1);

        for (i=start; i<=end; i++)
        {
            spx_word16_t g;
            spx_word32_t tmp;
            tmp = corr16[i-start];
            if (tmp>0)
            {
                if (SHR16(corr16[i-start],4)>ener16[i-start])
                    tmp = SHL32(EXTEND32(ener16[i-start]),14);
                else if (-SHR16(corr16[i-start],4)>ener16[i-start])
                    tmp = -SHL32(EXTEND32(ener16[i-start]),14);
                else
                    tmp = SHL32(tmp,10);
                g = DIV32_16(tmp, 8+ener16[i-start]);
                score[i-start] = MULT16_16(corr16[i-start],g);
            } else
            {
                score[i-start] = 1;
            }
        }
    }
#else
    for (i=start; i<=end; i++)
    {
        float g = corr[i-start]/(1+energy[i-start]);
        if (g>16)
            g = 16;
        else if (g<-16)
            g = -16;
        score[i-start] = g*corr[i-start];
    }
#endif

    /* Extract best scores */
    for (i=start; i<=end; i++)
    {
        if (score[i-start]>best_score[N-1])
        {
            for (j=0; j<N; j++)
            {
                if (score[i-start] > best_score[j])
                {
                    for (k=N-1; k>j; k--)
                    {
                        best_score[k]=best_score[k-1];
                        pitch[k]=pitch[k-1];
                    }
                    best_score[j]=score[i-start];
                    pitch[j]=i;
                    break;
                }
            }
        }
    }

    /* Compute open-loop gain */
    if (gain)
    {
        for (j=0; j<N; j++)
        {
            spx_word16_t g;
            i=pitch[j];
            g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
            /* FIXME: g = max(g,corr/energy) */
            if (g<0)
                g = 0;
            gain[j]=g;
        }
    }
}
Пример #17
0
/** Performs echo cancellation on a frame */
EXPORT void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
{
   int i,j, chan, speak;
   int N,M, C, K;
   spx_word32_t Syy,See,Sxx,Sdd, Sff;
#ifdef TWO_PATH
   spx_word32_t Dbf;
   int update_foreground;
#endif
   spx_word32_t Sey;
   spx_word16_t ss, ss_1;
   spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
   spx_float_t alpha, alpha_1;
   spx_word16_t RER;
   spx_word32_t tmp32;
   
   N = st->window_size;
   M = st->M;
   C = st->C;
   K = st->K;

   st->cancel_count++;
#ifdef FIXED_POINT
   ss=DIV32_16(11469,M);
   ss_1 = SUB16(32767,ss);
#else
   ss=.35/M;
   ss_1 = 1-ss;
#endif

   for (chan = 0; chan < C; chan++)
   {
      /* Apply a notch filter to make sure DC doesn't end up causing problems */
      filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C);
      /* Copy input data to buffer and apply pre-emphasis */
      /* Copy input data to buffer */
      for (i=0;i<st->frame_size;i++)
      {
         spx_word32_t tmp32;
         /* FIXME: This core has changed a bit, need to merge properly */
         tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan])));
#ifdef FIXED_POINT
         if (tmp32 > 32767)
         {
            tmp32 = 32767;
            if (st->saturated == 0)
               st->saturated = 1;
         }      
         if (tmp32 < -32767)
         {
            tmp32 = -32767;
            if (st->saturated == 0)
               st->saturated = 1;
         }
#endif
         st->memD[chan] = st->input[chan*st->frame_size+i];
         st->input[chan*st->frame_size+i] = EXTRACT16(tmp32);
      }
   }

   for (speak = 0; speak < K; speak++)
   {
      for (i=0;i<st->frame_size;i++)
      {
         spx_word32_t tmp32;
         st->x[speak*N+i] = st->x[speak*N+i+st->frame_size];
         tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak])));
#ifdef FIXED_POINT
         /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */
         if (tmp32 > 32767)
         {
            tmp32 = 32767;
            st->saturated = M+1;
         }      
         if (tmp32 < -32767)
         {
            tmp32 = -32767;
            st->saturated = M+1;
         }      
#endif
         st->x[speak*N+i+st->frame_size] = EXTRACT16(tmp32);
         st->memX[speak] = far_end[i*K+speak];
      }
   }   
   
   for (speak = 0; speak < K; speak++)
   {
      /* Shift memory: this could be optimized eventually*/
      for (j=M-1;j>=0;j--)
      {
         for (i=0;i<N;i++)
            st->X[(j+1)*N*K+speak*N+i] = st->X[j*N*K+speak*N+i];
      }
      /* Convert x (echo input) to frequency domain */
      spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]);
   }
   
   Sxx = 0;
   for (speak = 0; speak < K; speak++)
   {
      Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
      power_spectrum_accum(st->X+speak*N, st->Xf, N);
   }
   
   Sff = 0;  
   for (chan = 0; chan < C; chan++)
   {
#ifdef TWO_PATH
      /* Compute foreground filter */
      spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K);
      spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N);
      for (i=0;i<st->frame_size;i++)
         st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]);
      Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
#endif
   }
   
   /* Adjust proportional adaption rate */
   /* FIXME: Adjust that for C, K*/
   if (st->adapted)
      mdf_adjust_prop (st->W, N, M, C*K, st->prop);
   /* Compute weight gradient */
   if (st->saturated == 0)
   {
      for (chan = 0; chan < C; chan++)
      {
         for (speak = 0; speak < K; speak++)
         {
            for (j=M-1;j>=0;j--)
            {
               weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N);
               for (i=0;i<N;i++)
                  st->W[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i];
            }
         }
      }
   } else {
      st->saturated--;
   }
   
   /* FIXME: MC conversion required */ 
   /* Update weight to prevent circular convolution (MDF / AUMDF) */
   for (chan = 0; chan < C; chan++)
   {
      for (speak = 0; speak < K; speak++)
      {
         for (j=0;j<M;j++)
         {
            /* This is a variant of the Alternatively Updated MDF (AUMDF) */
            /* Remove the "if" to make this an MDF filter */
            if (j==0 || st->cancel_count%(M-1) == j-1)
            {
#ifdef FIXED_POINT
               for (i=0;i<N;i++)
                  st->wtmp2[i] = EXTRACT16(PSHR32(st->W[chan*N*K*M + j*N*K + speak*N + i],NORMALIZE_SCALEDOWN+16));
               spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
               for (i=0;i<st->frame_size;i++)
               {
                  st->wtmp[i]=0;
               }
               for (i=st->frame_size;i<N;i++)
               {
                  st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
               }
               spx_fft(st->fft_table, st->wtmp, st->wtmp2);
               /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
               for (i=0;i<N;i++)
                  st->W[chan*N*K*M + j*N*K + speak*N + i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
#else
               spx_ifft(st->fft_table, &st->W[chan*N*K*M + j*N*K + speak*N], st->wtmp);
               for (i=st->frame_size;i<N;i++)
               {
                  st->wtmp[i]=0;
               }
               spx_fft(st->fft_table, st->wtmp, &st->W[chan*N*K*M + j*N*K + speak*N]);
#endif
            }
         }
      }
   }
   
   /* So we can use power_spectrum_accum */ 
   for (i=0;i<=st->frame_size;i++)
      st->Rf[i] = st->Yf[i] = st->Xf[i] = 0;
      
   Dbf = 0;
   See = 0;    
#ifdef TWO_PATH
   /* Difference in response, this is used to estimate the variance of our residual power estimate */
   for (chan = 0; chan < C; chan++)
   {
      spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K);
      spx_ifft(st->fft_table, st->Y+chan*N, st->y+chan*N);
      for (i=0;i<st->frame_size;i++)
         st->e[chan*N+i] = SUB16(st->e[chan*N+i+st->frame_size], st->y[chan*N+i+st->frame_size]);
      Dbf += 10+mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
      for (i=0;i<st->frame_size;i++)
         st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]);
      See += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
   }
#endif

#ifndef TWO_PATH
   Sff = See;
#endif

#ifdef TWO_PATH
   /* Logic for updating the foreground filter */
   
   /* For two time windows, compute the mean of the energy difference, as well as the variance */
   st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See)));
   st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See)));
   st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf)));
   st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf)));
   
   /* Equivalent float code:
   st->Davg1 = .6*st->Davg1 + .4*(Sff-See);
   st->Davg2 = .85*st->Davg2 + .15*(Sff-See);
   st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf;
   st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf;
   */
   
   update_foreground = 0;
   /* Check if we have a statistically significant reduction in the residual echo */
   /* Note that this is *not* Gaussian, so we need to be careful about the longer tail */
   if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf)))
      update_foreground = 1;
   else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1))))
      update_foreground = 1;
   else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2))))
      update_foreground = 1;
   
   /* Do we update? */
   if (update_foreground)
   {
      st->Davg1 = st->Davg2 = 0;
      st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
      /* Copy background filter to foreground filter */
      for (i=0;i<N*M*C*K;i++)
         st->foreground[i] = EXTRACT16(PSHR32(st->W[i],16));
      /* Apply a smooth transition so as to not introduce blocking artifacts */
      for (chan = 0; chan < C; chan++)
         for (i=0;i<st->frame_size;i++)
            st->e[chan*N+i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]);
   } else {
      int reset_background=0;
      /* Otherwise, check if the background filter is significantly worse */
      if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf))))
         reset_background = 1;
      if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1)))
         reset_background = 1;
      if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2)))
         reset_background = 1;
      if (reset_background)
      {
         /* Copy foreground filter to background filter */
         for (i=0;i<N*M*C*K;i++)
            st->W[i] = SHL32(EXTEND32(st->foreground[i]),16);
         /* We also need to copy the output so as to get correct adaptation */
         for (chan = 0; chan < C; chan++)
         {        
            for (i=0;i<st->frame_size;i++)
               st->y[chan*N+i+st->frame_size] = st->e[chan*N+i+st->frame_size];
            for (i=0;i<st->frame_size;i++)
               st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]);
         }        
         See = Sff;
         st->Davg1 = st->Davg2 = 0;
         st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
      }
   }
#endif

   Sey = Syy = Sdd = 0;  
   for (chan = 0; chan < C; chan++)
   {    
      /* Compute error signal (for the output with de-emphasis) */ 
      for (i=0;i<st->frame_size;i++)
      {
         spx_word32_t tmp_out;
#ifdef TWO_PATH
         tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->e[chan*N+i+st->frame_size]));
#else
         tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->y[chan*N+i+st->frame_size]));
#endif
         tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE[chan])));
      /* This is an arbitrary test for saturation in the microphone signal */
         if (in[i*C+chan] <= -32000 || in[i*C+chan] >= 32000)
         {
         if (st->saturated == 0)
            st->saturated = 1;
         }
         out[i*C+chan] = WORD2INT(tmp_out);
         st->memE[chan] = tmp_out;
      }

#ifdef DUMP_ECHO_CANCEL_DATA
      dump_audio(in, far_end, out, st->frame_size);
#endif
   
      /* Compute error signal (filter update version) */ 
      for (i=0;i<st->frame_size;i++)
      {
         st->e[chan*N+i+st->frame_size] = st->e[chan*N+i];
         st->e[chan*N+i] = 0;
      }
      
      /* Compute a bunch of correlations */
      /* FIXME: bad merge */
      Sey += mdf_inner_prod(st->e+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size);
      Syy += mdf_inner_prod(st->y+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size);
      Sdd += mdf_inner_prod(st->input+chan*st->frame_size, st->input+chan*st->frame_size, st->frame_size);
      
      /* Convert error to frequency domain */
      spx_fft(st->fft_table, st->e+chan*N, st->E+chan*N);
      for (i=0;i<st->frame_size;i++)
         st->y[i+chan*N] = 0;
      spx_fft(st->fft_table, st->y+chan*N, st->Y+chan*N);
   
      /* Compute power spectrum of echo (X), error (E) and filter response (Y) */
      power_spectrum_accum(st->E+chan*N, st->Rf, N);
      power_spectrum_accum(st->Y+chan*N, st->Yf, N);
    
   }
   
   /*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/
   
   /* Do some sanity check */
   if (!(Syy>=0 && Sxx>=0 && See >= 0)
#ifndef FIXED_POINT
       || !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9)
#endif
      )
   {
      /* Things have gone really bad */
      st->screwed_up += 50;
      for (i=0;i<st->frame_size*C;i++)
         out[i] = 0;
   } else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6)))
   {
      /* AEC seems to add lots of echo instead of removing it, let's see if it will improve */
      st->screwed_up++;
   } else {
      /* Everything's fine */
      st->screwed_up=0;
   }
   if (st->screwed_up>=50)
   {
      speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now.");
      speex_echo_state_reset(st);
      return;
   }

   /* Add a small noise floor to make sure not to have problems when dividing */
   See = MAX32(See, SHR32(MULT16_16(N, 100),6));
     
   for (speak = 0; speak < K; speak++)
   {
      Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
      power_spectrum_accum(st->X+speak*N, st->Xf, N);
   }

   
   /* Smooth far end energy estimate over time */
   for (j=0;j<=st->frame_size;j++)
      st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]);

   /* Compute filtered spectra and (cross-)correlations */
   for (j=st->frame_size;j>=0;j--)
   {
      spx_float_t Eh, Yh;
      Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]);
      Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]);
      Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));
      Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));
#ifdef FIXED_POINT
      st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]);
      st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]);
#else
      st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j];
      st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j];
#endif
   }
   
   Pyy = FLOAT_SQRT(Pyy);
   Pey = FLOAT_DIVU(Pey,Pyy);

   /* Compute correlation updatete rate */
   tmp32 = MULT16_32_Q15(st->beta0,Syy);
   if (tmp32 > MULT16_32_Q15(st->beta_max,See))
      tmp32 = MULT16_32_Q15(st->beta_max,See);
   alpha = FLOAT_DIV32(tmp32, See);
   alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha);
   /* Update correlations (recursive average) */
   st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey));
   st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy));
   if (FLOAT_LT(st->Pyy, FLOAT_ONE))
      st->Pyy = FLOAT_ONE;
   /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */
   if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy)))
      st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy);
   if (FLOAT_GT(st->Pey, st->Pyy))
      st->Pey = st->Pyy;
   /* leak_estimate is the linear regression result */
   st->leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14));
   /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */
   if (st->leak_estimate > 16383)
      st->leak_estimate = 32767;
   else
      st->leak_estimate = SHL16(st->leak_estimate,1);
   /*printf ("%f\n", st->leak_estimate);*/
   
   /* Compute Residual to Error Ratio */
#ifdef FIXED_POINT
   tmp32 = MULT16_32_Q15(st->leak_estimate,Syy);
   tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1)));
   /* Check for y in e (lower bound on RER) */
   {
      spx_float_t bound = PSEUDOFLOAT(Sey);
      bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy)));
      if (FLOAT_GT(bound, PSEUDOFLOAT(See)))
         tmp32 = See;
      else if (tmp32 < FLOAT_EXTRACT32(bound))
         tmp32 = FLOAT_EXTRACT32(bound);
   }
   if (tmp32 > SHR32(See,1))
      tmp32 = SHR32(See,1);
   RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15));
#else
   RER = (.0001*Sxx + 3.*MULT16_32_Q15(st->leak_estimate,Syy)) / See;
   /* Check for y in e (lower bound on RER) */
   if (RER < Sey*Sey/(1+See*Syy))
      RER = Sey*Sey/(1+See*Syy);
   if (RER > .5)
      RER = .5;
#endif

   /* We consider that the filter has had minimal adaptation if the following is true*/
   if (!st->adapted && st->sum_adapt > SHL32(EXTEND32(M),15) && MULT16_32_Q15(st->leak_estimate,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy))
   {
      st->adapted = 1;
   }

   if (st->adapted)
   {
      /* Normal learning rate calculation once we're past the minimal adaptation phase */
      for (i=0;i<=st->frame_size;i++)
      {
         spx_word32_t r, e;
         /* Compute frequency-domain adaptation mask */
         r = MULT16_32_Q15(st->leak_estimate,SHL32(st->Yf[i],3));
         e = SHL32(st->Rf[i],3)+1;
#ifdef FIXED_POINT
         if (r>SHR32(e,1))
            r = SHR32(e,1);
#else
         if (r>.5*e)
            r = .5*e;
#endif
         r = MULT16_32_Q15(QCONST16(.7,15),r) + MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));
         /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/
         st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16);
      }
   } else {
      /* Temporary adaption rate if filter is not yet adapted enough */
      spx_word16_t adapt_rate=0;

      if (Sxx > SHR32(MULT16_16(N, 1000),6)) 
      {
         tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx);
#ifdef FIXED_POINT
         if (tmp32 > SHR32(See,2))
            tmp32 = SHR32(See,2);
#else
         if (tmp32 > .25*See)
            tmp32 = .25*See;
#endif
         adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15));
      }
      for (i=0;i<=st->frame_size;i++)
         st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1);


      /* How much have we adapted so far? */
      st->sum_adapt = ADD32(st->sum_adapt,adapt_rate);
   }

   /* FIXME: MC conversion required */ 
      for (i=0;i<st->frame_size;i++)
         st->last_y[i] = st->last_y[st->frame_size+i];
   if (st->adapted)
   {
      /* If the filter is adapted, take the filtered echo */
      for (i=0;i<st->frame_size;i++)
         st->last_y[st->frame_size+i] = in[i]-out[i];
   } else {
      /* If filter isn't adapted yet, all we can do is take the far end signal directly */
      /* moved earlier: for (i=0;i<N;i++)
      st->last_y[i] = st->x[i];*/
   }

}
Пример #18
0
void split_cb_search_shape_sign(
spx_sig_t target[],			/* target vector */
spx_coef_t ak[],			/* LPCs for this subframe */
spx_coef_t awk1[],			/* Weighted LPCs for this subframe */
spx_coef_t awk2[],			/* Weighted LPCs for this subframe */
const void *par,                      /* Codebook/search parameters*/
int   p,                        /* number of LPC coeffs */
int   nsf,                      /* number of samples in subframe */
spx_sig_t *exc,
spx_sig_t *r,
SpeexBits *bits,
char *stack,
int   complexity
)
{
   int i,j,k,m,n,q;
   spx_word16_t *resp;
#ifdef _USE_SSE
   __m128 *resp2;
   __m128 *E;
#else
   spx_word16_t *resp2;
   spx_word32_t *E;
#endif
   spx_word16_t *t;
   spx_sig_t *e, *r2;
   spx_word16_t *tmp;
   spx_word32_t *ndist, *odist;
   int *itmp;
   spx_word16_t **ot, **nt;
   int **nind, **oind;
   int *ind;
   const signed char *shape_cb;
   int shape_cb_size, subvect_size, nb_subvect;
   split_cb_params *params;
   int N=2;
   int *best_index;
   spx_word32_t *best_dist;
   int have_sign;
   N=complexity;
   if (N>10)
      N=10;

   ot=PUSH(stack, N, spx_word16_t*);
   nt=PUSH(stack, N, spx_word16_t*);
   oind=PUSH(stack, N, int*);
   nind=PUSH(stack, N, int*);

   params = (split_cb_params *) par;
   subvect_size = params->subvect_size;
   nb_subvect = params->nb_subvect;
   shape_cb_size = 1<<params->shape_bits;
   shape_cb = params->shape_cb;
   have_sign = params->have_sign;
   resp = PUSH(stack, shape_cb_size*subvect_size, spx_word16_t);
#ifdef _USE_SSE
   resp2 = PUSH(stack, (shape_cb_size*subvect_size)>>2, __m128);
   E = PUSH(stack, shape_cb_size>>2, __m128);
#else
   resp2 = resp;
   E = PUSH(stack, shape_cb_size, spx_word32_t);
#endif
   t = PUSH(stack, nsf, spx_word16_t);
   e = PUSH(stack, nsf, spx_sig_t);
   r2 = PUSH(stack, nsf, spx_sig_t);
   ind = PUSH(stack, nb_subvect, int);

   tmp = PUSH(stack, 2*N*nsf, spx_word16_t);
   for (i=0;i<N;i++)
   {
      ot[i]=tmp;
      tmp += nsf;
      nt[i]=tmp;
      tmp += nsf;
   }
   best_index = PUSH(stack, N, int);
   best_dist = PUSH(stack, N, spx_word32_t);
   ndist = PUSH(stack, N, spx_word32_t);
   odist = PUSH(stack, N, spx_word32_t);
   
   itmp = PUSH(stack, 2*N*nb_subvect, int);
   for (i=0;i<N;i++)
   {
      nind[i]=itmp;
      itmp+=nb_subvect;
      oind[i]=itmp;
      itmp+=nb_subvect;
      for (j=0;j<nb_subvect;j++)
         nind[i][j]=oind[i][j]=-1;
   }
   
   /* FIXME: make that adaptive? */
   for (i=0;i<nsf;i++)
      t[i]=SHR(target[i],6);

   for (j=0;j<N;j++)
      for (i=0;i<nsf;i++)
         ot[j][i]=t[i];

   /*for (i=0;i<nsf;i++)
     printf ("%d\n", (int)t[i]);*/

   /* Pre-compute codewords response and energy */
   compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);

   for (j=0;j<N;j++)
      odist[j]=0;
   /*For all subvectors*/
   for (i=0;i<nb_subvect;i++)
   {
      /*"erase" nbest list*/
      for (j=0;j<N;j++)
         ndist[j]=-2;

      /*For all n-bests of previous subvector*/
      for (j=0;j<N;j++)
      {
         spx_word16_t *x=ot[j]+subvect_size*i;
         /*Find new n-best based on previous n-best j*/
         if (have_sign)
            vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
         else
            vq_nbest(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);

         /*For all new n-bests*/
         for (k=0;k<N;k++)
         {
            spx_word16_t *ct;
            spx_word32_t err=0;
            ct = ot[j];
            /*update target*/

            /*previous target*/
            for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
               t[m]=ct[m];

            /* New code: update only enough of the target to calculate error*/
            {
               int rind;
               spx_word16_t *res;
               spx_word16_t sign=1;
               rind = best_index[k];
               if (rind>=shape_cb_size)
               {
                  sign=-1;
                  rind-=shape_cb_size;
               }
               res = resp+rind*subvect_size;
               if (sign>0)
                  for (m=0;m<subvect_size;m++)
                     t[subvect_size*i+m] -= res[m];
               else
                  for (m=0;m<subvect_size;m++)
                     t[subvect_size*i+m] += res[m];
            }
            
            /*compute error (distance)*/
            err=odist[j];
            for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
               err += t[m]*t[m];
            /*update n-best list*/
            if (err<ndist[N-1] || ndist[N-1]<-1)
            {

               /*previous target (we don't care what happened before*/
               for (m=(i+1)*subvect_size;m<nsf;m++)
                  t[m]=ct[m];
               /* New code: update the rest of the target only if it's worth it */
               for (m=0;m<subvect_size;m++)
               {
                  spx_word16_t g;
                  int rind;
                  spx_word16_t sign=1;
                  rind = best_index[k];
                  if (rind>=shape_cb_size)
                  {
                     sign=-1;
                     rind-=shape_cb_size;
                  }

                  q=subvect_size-m;
#ifdef FIXED_POINT
                  g=sign*shape_cb[rind*subvect_size+m];
                  for (n=subvect_size*(i+1);n<nsf;n++,q++)
                     t[n] = SUB32(t[n],MULT16_16_Q11(g,r[q]));
#else
                  g=sign*0.03125*shape_cb[rind*subvect_size+m];
                  for (n=subvect_size*(i+1);n<nsf;n++,q++)
                     t[n] = SUB32(t[n],g*r[q]);
#endif
               }


               for (m=0;m<N;m++)
               {
                  if (err < ndist[m] || ndist[m]<-1)
                  {
                     for (n=N-1;n>m;n--)
                     {
                        for (q=(i+1)*subvect_size;q<nsf;q++)
                           nt[n][q]=nt[n-1][q];
                        for (q=0;q<nb_subvect;q++)
                           nind[n][q]=nind[n-1][q];
                        ndist[n]=ndist[n-1];
                     }
                     for (q=(i+1)*subvect_size;q<nsf;q++)
                        nt[m][q]=t[q];
                     for (q=0;q<nb_subvect;q++)
                        nind[m][q]=oind[j][q];
                     nind[m][i]=best_index[k];
                     ndist[m]=err;
                     break;
                  }
               }
            }
         }
         if (i==0)
           break;
      }

      /*update old-new data*/
      /* just swap pointers instead of a long copy */
      {
         spx_word16_t **tmp2;
         tmp2=ot;
         ot=nt;
         nt=tmp2;
      }
      for (j=0;j<N;j++)
         for (m=0;m<nb_subvect;m++)
            oind[j][m]=nind[j][m];
      for (j=0;j<N;j++)
         odist[j]=ndist[j];
   }

   /*save indices*/
   for (i=0;i<nb_subvect;i++)
   {
      ind[i]=nind[0][i];
      speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
   }
   
   /* Put everything back together */
   for (i=0;i<nb_subvect;i++)
   {
      int rind;
      spx_word16_t sign=1;
      rind = ind[i];
      if (rind>=shape_cb_size)
      {
         sign=-1;
         rind-=shape_cb_size;
      }
#ifdef FIXED_POINT
      if (sign==1)
      {
         for (j=0;j<subvect_size;j++)
            e[subvect_size*i+j]=SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
      } else {
         for (j=0;j<subvect_size;j++)
            e[subvect_size*i+j]=-SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
      }
#else
      for (j=0;j<subvect_size;j++)
         e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
#endif
   }   
   /* Update excitation */
   for (j=0;j<nsf;j++)
      exc[j]+=e[j];
   
   /* Update target */
   syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
   for (j=0;j<nsf;j++)
      target[j]-=r2[j];

}
Пример #19
0
void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/*  float *freq 	array of LSP frequencies in the x domain	*/
/*  float *ak 		array of LPC coefficients 			*/
/*  int lpcrdr  	order of LPC coefficients 			*/


{
    int i,j;
    spx_word32_t xout1,xout2,xin1,xin2;
    spx_word32_t *Wp;
    spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL;
    spx_word16_t *freqn;
    int m = lpcrdr>>1;
    
    freqn = PUSH(stack, lpcrdr, spx_word16_t);
    for (i=0;i<lpcrdr;i++)
       freqn[i] = ANGLE2X(freq[i]);

    Wp = PUSH(stack, 4*m+2, spx_word32_t);
    pw = Wp;


    /* initialise contents of array */

    for(i=0;i<=4*m+1;i++){       	/* set contents of buffer to 0 */
	*pw++ = 0;
    }

    /* Set pointers up */

    pw = Wp;
    xin1 = 1048576;
    xin2 = 1048576;

    /* reconstruct P(z) and Q(z) by  cascading second order
      polynomials in form 1 - 2xz(-1) +z(-2), where x is the
      LSP coefficient */

    for(j=0;j<=lpcrdr;j++){
       spx_word16_t *fr=freqn;
	for(i=0;i<m;i++){
	    n1 = pw+(i<<2);
	    n2 = n1 + 1;
	    n3 = n2 + 1;
	    n4 = n3 + 1;
	    xout1 = ADD32(SUB32(xin1, MULT16_32_Q14(*fr,*n1)), *n2);
            fr++;
            xout2 = ADD32(SUB32(xin2, MULT16_32_Q14(*fr,*n3)), *n4);
            fr++;
	    *n2 = *n1;
	    *n4 = *n3;
	    *n1 = xin1;
	    *n3 = xin2;
	    xin1 = xout1;
	    xin2 = xout2;
	}
	xout1 = xin1 + *(n4+1);
	xout2 = xin2 - *(n4+2);
        /* FIXME: perhaps apply bandwidth expansion in case of overflow? */
        if (xout1 + xout2>256*32766)
           ak[j] = 32767;
        else if (xout1 + xout2 < -256*32767)
           ak[j] = -32768;
        else
           ak[j] = PSHR(ADD32(xout1,xout2),8);
	*(n4+1) = xin1;
	*(n4+2) = xin2;

	xin1 = 0;
	xin2 = 0;
    }
}
int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
/*  float *a                  lpc coefficients            */
/*  int lpcrdr            order of LPC coefficients (10)         */
/*  float *freq               LSP frequencies in the x domain           */
/*  int nb            number of sub-intervals (4)         */
/*  float delta            grid spacing interval (0.02)         */


{
    spx_word16_t temp_xr,xl,xr,xm=0;
    spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
    int i,j,m,flag,k;
#ifndef FIXED_LPC_SIZE
    VARDECL(spx_word32_t *Q);                     /* ptrs for memory allocation         */
    VARDECL(spx_word32_t *P);
    VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation         */
    VARDECL(spx_word16_t *P16);
#else
    spx_word32_t Q[(FIXED_LPC_SIZE/2)+1];         /* ptrs for memory allocation         */
    spx_word32_t P[(FIXED_LPC_SIZE/2)+1];
    spx_word16_t Q16[(FIXED_LPC_SIZE/2)+1];       /* ptrs for memory allocation         */
    spx_word16_t P16[(FIXED_LPC_SIZE/2)+1];
#endif
    spx_word32_t *px;                    /* ptrs of respective P'(z) & Q'(z)    */
    spx_word32_t *qx;
    spx_word32_t *p;
    spx_word32_t *q;
    spx_word16_t *pt;                    /* ptr used for cheb_poly_eval()
                whether P' or Q'             */
    int roots=0;                  /* DR 8/2/94: number of roots found     */
    flag = 1;                    /*  program is searching for a root when,
                1 else has found one             */
    m = lpcrdr/2;                /* order of P'(z) & Q'(z) polynomials     */

#ifndef FIXED_LPC_SIZE
    /* Allocate memory space for polynomials */
    ALLOC(Q, (m+1), spx_word32_t);
    ALLOC(P, (m+1), spx_word32_t);
#endif

    /* determine P'(z)'s and Q'(z)'s coefficients where
      P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */

    px = P;                      /* initialise ptrs             */
    qx = Q;
    p = px;
    q = qx;

#ifdef FIXED_POINT
    *px++ = LPC_SCALING;
    *qx++ = LPC_SCALING;
#ifndef FIXED_LPC_SIZE
    for(i=0;i<m;i++){
       *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
       *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
    }
#else
    for(i=0;i<(FIXED_LPC_SIZE/2);i++){
       *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[FIXED_LPC_SIZE-i-1])), *p++);
       *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[FIXED_LPC_SIZE-i-1])), *q++);
    }
#endif
    px = P;
    qx = Q;
    for(i=0;i<m;i++)
    {
       /*if (fabs(*px)>=32768)
          speex_warning_int("px", *px);
       if (fabs(*qx)>=32768)
       speex_warning_int("qx", *qx);*/
       *px = PSHR32(*px,2);
       *qx = PSHR32(*qx,2);
       px++;
       qx++;
    }
    /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
    P[m] = PSHR32(P[m],3);
    Q[m] = PSHR32(Q[m],3);
#else
    *px++ = LPC_SCALING;
    *qx++ = LPC_SCALING;
    for(i=0;i<m;i++){
       *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
       *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
    }
    px = P;
    qx = Q;
    for(i=0;i<m;i++){
       *px = 2**px;
       *qx = 2**qx;
       px++;
       qx++;
    }
#endif

    px = P;                 /* re-initialise ptrs             */
    qx = Q;

    /* now that we have computed P and Q convert to 16 bits to
       speed up cheb_poly_eval */

#ifndef FIXED_LPC_SIZE
    ALLOC(P16, m+1, spx_word16_t);
    ALLOC(Q16, m+1, spx_word16_t);
#endif

    for (i=0;i<m+1;i++)
    {
       P16[i] = P[i];
       Q16[i] = Q[i];
    }

    /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
    Keep alternating between the two polynomials as each zero is found     */

    xr = 0;                 /* initialise xr to zero         */
    xl = FREQ_SCALE;                   /* start at point xl = 1         */

    for(j=0;j<lpcrdr;j++){
    if(j&1)                /* determines whether P' or Q' is eval. */
        pt = Q16;
    else
        pt = P16;

    psuml = cheb_poly_eva(pt,xl,m,stack);    /* evals poly. at xl     */
    flag = 1;
    while(flag && (xr >= -FREQ_SCALE)){
           spx_word16_t dd;
           /* Modified by JMV to provide smaller steps around x=+-1 */
#ifdef FIXED_POINT
           dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
           if (psuml<512 && psuml>-512)
              dd = PSHR16(dd,1);
#else
           dd=delta*(1-.9*xl*xl);
           if (fabs(psuml)<.2)
              dd *= .5;
#endif
           xr = SUB16(xl, dd);                            /* interval spacing     */
        psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x)     */
        temp_psumr = psumr;
        temp_xr = xr;

    /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
    sign change.
    if a sign change has occurred the interval is bisected and then
    checked again for a sign change which determines in which
    interval the zero lies in.
    If there is no sign change between poly(xm) and poly(xl) set interval
    between xm and xr else set interval between xl and xr and repeat till
    root is located within the specified limits             */

        if(SIGN_CHANGE(psumr,psuml))
            {
        roots++;

        psumm=psuml;
        for(k=0;k<=nb;k++){
#ifdef FIXED_POINT
            xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));            /* bisect the interval     */
#else
                    xm = .5*(xl+xr);            /* bisect the interval     */
#endif
            psumm=cheb_poly_eva(pt,xm,m,stack);
            /*if(psumm*psuml>0.)*/
            if(!SIGN_CHANGE(psumm,psuml))
                    {
            psuml=psumm;
            xl=xm;
            } else {
            psumr=psumm;
            xr=xm;
            }
        }

           /* once zero is found, reset initial interval to xr     */
           freq[j] = X2ANGLE(xm);
           xl = xm;
           flag = 0;               /* reset flag for next search     */
        }
        else{
        psuml=temp_psumr;
        xl=temp_xr;
        }
    }
    }
    return(roots);
}
void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/*  float *freq     array of LSP frequencies in the x domain    */
/*  float *ak         array of LPC coefficients             */
/*  int lpcrdr      order of LPC coefficients             */
{
    int i,j;
    spx_word32_t xout1,xout2,xin;
    spx_word32_t mult, a;
    VARDECL(spx_word32_t *xpmem);
    VARDECL(spx_word32_t *xqmem);
#ifndef FIXED_LPC_SIZE
    VARDECL(spx_word16_t *freqn);
    VARDECL(spx_word32_t **xp);
    VARDECL(spx_word32_t **xq);
#else
    spx_word16_t freqn[FIXED_LPC_SIZE];
    spx_word32_t *xp[(FIXED_LPC_SIZE/2)+1];
    spx_word32_t *xq[(FIXED_LPC_SIZE/2)+1];
#endif

    int m = lpcrdr>>1;

    /*

       Reconstruct P(z) and Q(z) by cascading second order polynomials
       in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
       In the time domain this is:

       y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)

       This is what the ALLOCS below are trying to do:

         int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
         int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP

       These matrices store the output of each stage on each row.  The
       final (m-th) row has the output of the final (m-th) cascaded
       2nd order filter.  The first row is the impulse input to the
       system (not written as it is known).

       The version below takes advantage of the fact that a lot of the
       outputs are zero or known, for example if we put an inpulse
       into the first section the "clock" it 10 times only the first 3
       outputs samples are non-zero (it's an FIR filter).
    */

#ifndef FIXED_LPC_SIZE
    ALLOC(xp, (m+1), spx_word32_t*);
#endif
    ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);

#ifndef FIXED_LPC_SIZE
    ALLOC(xq, (m+1), spx_word32_t*);
#endif
    ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);

#ifndef FIXED_LPC_SIZE
    for(i=0; i<=m; i++) {
      xp[i] = xpmem + i*(lpcrdr+1+2);
      xq[i] = xqmem + i*(lpcrdr+1+2);
    }
#else
    for(i=0; i<=m; i++) {
      xp[i] = xpmem + i*(FIXED_LPC_SIZE+1+2);
      xq[i] = xqmem + i*(FIXED_LPC_SIZE+1+2);
    }
#endif
    /* work out 2cos terms in Q14 */

#ifndef FIXED_LPC_SIZE
    ALLOC(freqn, lpcrdr, spx_word16_t);
    for (i=0;i<lpcrdr;i++)
       freqn[i] = ANGLE2X(freq[i]);
#else
    for (i=0;i<FIXED_LPC_SIZE;i++)
       freqn[i] = ANGLE2X(freq[i]);
#endif

    #define QIMP  21   /* scaling for impulse */

    xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */

    /* first col and last non-zero values of each row are trivial */

    for(i=0;i<=m;i++) {
     xp[i][1] = 0;
     xp[i][2] = xin;
     xp[i][2+2*i] = xin;
     xq[i][1] = 0;
     xq[i][2] = xin;
     xq[i][2+2*i] = xin;
    }

    /* 2nd row (first output row) is trivial */

    xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
    xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);

    xout1 = xout2 = 0;

    /* now generate remaining rows */

    for(i=1;i<m;i++) {

      for(j=1;j<2*(i+1)-1;j++) {
    mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
    xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
    mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
    xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
      }

      /* for last col xp[i][j+2] = xq[i][j+2] = 0 */

      mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
      xp[i+1][j+2] = SUB32(xp[i][j], mult);
      mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
      xq[i+1][j+2] = SUB32(xq[i][j], mult);
    }

    /* process last row to extra a{k} */

#ifndef FIXED_LPC_SIZE
    for(j=1;j<=lpcrdr;j++) {
#else
    for(j=1;j<=FIXED_LPC_SIZE;j++) {
#endif
      int shift = QIMP-13;

      /* final filter sections */
      a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);
      xout1 = xp[m][j+2];
      xout2 = xq[m][j+2];

      /* hard limit ak's to +/- 32767 */

      if (a < -32767) a = -32767;
      if (a > 32767) a = 32767;
      ak[j-1] = (short)a;

    }

}

#else

void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/*  float *freq     array of LSP frequencies in the x domain    */
/*  float *ak         array of LPC coefficients             */
/*  int lpcrdr      order of LPC coefficients             */


{
    int i,j;
    float xout1,xout2,xin1,xin2;
    VARDECL(float *Wp);
    float *pw,*n1,*n2,*n3,*n4=NULL;
    VARDECL(float *x_freq);
    int m = lpcrdr>>1;

    ALLOC(Wp, 4*m+2, float);
    pw = Wp;

    /* initialise contents of array */

    for(i=0;i<=4*m+1;i++){           /* set contents of buffer to 0 */
    *pw++ = 0.0;
    }

    /* Set pointers up */

    pw = Wp;
    xin1 = 1.0;
    xin2 = 1.0;

    ALLOC(x_freq, lpcrdr, float);
    for (i=0;i<lpcrdr;i++)
       x_freq[i] = ANGLE2X(freq[i]);

    /* reconstruct P(z) and Q(z) by  cascading second order
      polynomials in form 1 - 2xz(-1) +z(-2), where x is the
      LSP coefficient */

    for(j=0;j<=lpcrdr;j++){
       int i2=0;
    for(i=0;i<m;i++,i2+=2){
        n1 = pw+(i*4);
        n2 = n1 + 1;
        n3 = n2 + 1;
        n4 = n3 + 1;
        xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
        xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
        *n2 = *n1;
        *n4 = *n3;
        *n1 = xin1;
        *n3 = xin2;
        xin1 = xout1;
        xin2 = xout2;
    }
    xout1 = xin1 + *(n4+1);
    xout2 = xin2 - *(n4+2);
    if (j>0)
       ak[j-1] = (xout1 + xout2)*0.5f;
    *(n4+1) = xin1;
    *(n4+2) = xin2;

    xin1 = 0.0;
    xin2 = 0.0;
    }

}
Пример #22
0
void DEC32(reg_t r1)
{
  SUB32(r1, r1, R_ONE);
}
Пример #23
0
Файл: ltp.c Проект: Affix/fgcom
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
static spx_word64_t pitch_gain_search_3tap(
    const spx_sig_t target[],       /* Target vector */
    const spx_coef_t ak[],          /* LPCs for this subframe */
    const spx_coef_t awk1[],        /* Weighted LPCs #1 for this subframe */
    const spx_coef_t awk2[],        /* Weighted LPCs #2 for this subframe */
    spx_sig_t exc[],                /* Excitation */
    const void *par,
    int   pitch,                    /* Pitch value */
    int   p,                        /* Number of LPC coeffs */
    int   nsf,                      /* Number of samples in subframe */
    SpeexBits *bits,
    char *stack,
    const spx_sig_t *exc2,
    const spx_word16_t *r,
    spx_sig_t *new_target,
    int  *cdbk_index,
    int cdbk_offset,
    int plc_tuning
)
{
    int i,j;
    VARDECL(spx_sig_t *tmp1);
    VARDECL(spx_sig_t *tmp2);
    spx_sig_t *x[3];
    spx_sig_t *e[3];
    spx_word32_t corr[3];
    spx_word32_t A[3][3];
    int   gain_cdbk_size;
    const signed char *gain_cdbk;
    spx_word16_t gain[3];
    spx_word64_t err;

    const ltp_params *params;
    params = (const ltp_params*) par;
    gain_cdbk_size = 1<<params->gain_bits;
    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
    ALLOC(tmp1, 3*nsf, spx_sig_t);
    ALLOC(tmp2, 3*nsf, spx_sig_t);

    x[0]=tmp1;
    x[1]=tmp1+nsf;
    x[2]=tmp1+2*nsf;

    e[0]=tmp2;
    e[1]=tmp2+nsf;
    e[2]=tmp2+2*nsf;
    for (i=2; i>=0; i--)
    {
        int pp=pitch+1-i;
        for (j=0; j<nsf; j++)
        {
            if (j-pp<0)
                e[i][j]=exc2[j-pp];
            else if (j-pp-pitch<0)
                e[i][j]=exc2[j-pp-pitch];
            else
                e[i][j]=0;
        }

        if (i==2)
            syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
        else {
            for (j=0; j<nsf-1; j++)
                x[i][j+1]=x[i+1][j];
            x[i][0]=0;
            for (j=0; j<nsf; j++)
            {
                x[i][j]=ADD32(x[i][j],SHL32(MULT16_32_Q15(r[j], e[i][0]),1));
            }
        }
    }

#ifdef FIXED_POINT
    {
        /* If using fixed-point, we need to normalize the signals first */
        spx_word16_t *y[3];
        VARDECL(spx_word16_t *ytmp);
        VARDECL(spx_word16_t *t);

        spx_sig_t max_val=1;
        int sig_shift;

        ALLOC(ytmp, 3*nsf, spx_word16_t);
#if 0
        ALLOC(y[0], nsf, spx_word16_t);
        ALLOC(y[1], nsf, spx_word16_t);
        ALLOC(y[2], nsf, spx_word16_t);
#else
        y[0] = ytmp;
        y[1] = ytmp+nsf;
        y[2] = ytmp+2*nsf;
#endif
        ALLOC(t, nsf, spx_word16_t);
        for (j=0; j<3; j++)
        {
            for (i=0; i<nsf; i++)
            {
                spx_sig_t tmp = x[j][i];
                if (tmp<0)
                    tmp = -tmp;
                if (tmp > max_val)
                    max_val = tmp;
            }
        }
        for (i=0; i<nsf; i++)
        {
            spx_sig_t tmp = target[i];
            if (tmp<0)
                tmp = -tmp;
            if (tmp > max_val)
                max_val = tmp;
        }

        sig_shift=0;
        while (max_val>16384)
        {
            sig_shift++;
            max_val >>= 1;
        }

        for (j=0; j<3; j++)
        {
            for (i=0; i<nsf; i++)
            {
                y[j][i] = EXTRACT16(SHR32(x[j][i],sig_shift));
            }
        }
        for (i=0; i<nsf; i++)
        {
            t[i] = EXTRACT16(SHR32(target[i],sig_shift));
        }

        for (i=0; i<3; i++)
            corr[i]=inner_prod(y[i],t,nsf);

        for (i=0; i<3; i++)
            for (j=0; j<=i; j++)
                A[i][j]=A[j][i]=inner_prod(y[i],y[j],nsf);
    }
#else
    {
        for (i=0; i<3; i++)
            corr[i]=inner_prod(x[i],target,nsf);

        for (i=0; i<3; i++)
            for (j=0; j<=i; j++)
                A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
    }
#endif

    {
        spx_word32_t C[9];
        const signed char *ptr=gain_cdbk;
        int best_cdbk=0;
        spx_word32_t best_sum=0;
        C[0]=corr[2];
        C[1]=corr[1];
        C[2]=corr[0];
        C[3]=A[1][2];
        C[4]=A[0][1];
        C[5]=A[0][2];
        C[6]=A[2][2];
        C[7]=A[1][1];
        C[8]=A[0][0];

        /*plc_tuning *= 2;*/
        if (plc_tuning<2)
            plc_tuning=2;
#ifdef FIXED_POINT
        C[0] = MAC16_32_Q15(C[0],MULT16_16_16(plc_tuning,-327),C[0]);
        C[1] = MAC16_32_Q15(C[1],MULT16_16_16(plc_tuning,-327),C[1]);
        C[2] = MAC16_32_Q15(C[2],MULT16_16_16(plc_tuning,-327),C[2]);
#else
        C[0]*=1-.01*plc_tuning;
        C[1]*=1-.01*plc_tuning;
        C[2]*=1-.01*plc_tuning;
        C[6]*=.5*(1+.01*plc_tuning);
        C[7]*=.5*(1+.01*plc_tuning);
        C[8]*=.5*(1+.01*plc_tuning);
#endif
        for (i=0; i<gain_cdbk_size; i++)
        {
            spx_word32_t sum=0;
            spx_word16_t g0,g1,g2;
            spx_word16_t pitch_control=64;
            spx_word16_t gain_sum;

            ptr = gain_cdbk+3*i;
            g0=ADD16((spx_word16_t)ptr[0],32);
            g1=ADD16((spx_word16_t)ptr[1],32);
            g2=ADD16((spx_word16_t)ptr[2],32);

            gain_sum = g1;
            if (g0>0)
                gain_sum += g0;
            if (g2>0)
                gain_sum += g2;
            if (gain_sum > 64)
            {
                gain_sum = SUB16(gain_sum, 64);
                if (gain_sum > 127)
                    gain_sum = 127;
#ifdef FIXED_POINT
                pitch_control =  SUB16(64,EXTRACT16(PSHR32(MULT16_16(64,MULT16_16_16(plc_tuning, gain_sum)),10)));
#else
                pitch_control = 64*(1.-.001*plc_tuning*gain_sum);
#endif
                if (pitch_control < 0)
                    pitch_control = 0;
            }

            sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g0,pitch_control),C[0]));
            sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g1,pitch_control),C[1]));
            sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g2,pitch_control),C[2]));
            sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g0,g1),C[3]));
            sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g1),C[4]));
            sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g0),C[5]));
            sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g0,g0),C[6]));
            sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g1,g1),C[7]));
            sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g2,g2),C[8]));
            /* We could force "safe" pitch values to handle packet loss better */

            if (sum>best_sum || i==0)
            {
                best_sum=sum;
                best_cdbk=i;
            }
        }
#ifdef FIXED_POINT
        gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3]);
        gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+1]);
        gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+2]);
        /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
        gain[0] = 0.015625*gain_cdbk[best_cdbk*3]  + .5;
        gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
        gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
#endif
        *cdbk_index=best_cdbk;
    }

#ifdef FIXED_POINT
    for (i=0; i<nsf; i++)
        exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])),
                           MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2);

    err=0;
    for (i=0; i<nsf; i++)
    {
        spx_word16_t perr2;
        spx_sig_t tmp = SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),x[2][i]),MULT16_32_Q15(SHL16(gain[1],7),x[1][i])),
                                    MULT16_32_Q15(SHL16(gain[2],7),x[0][i])),2);
        spx_sig_t perr=SUB32(target[i],tmp);
        new_target[i] = SUB32(target[i], tmp);
        perr2 = EXTRACT16(PSHR32(perr,15));
        err = ADD64(err,MULT16_16(perr2,perr2));

    }
#else
    for (i=0; i<nsf; i++)
        exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];

    err=0;
    for (i=0; i<nsf; i++)
    {
        spx_sig_t tmp = gain[2]*x[0][i]+gain[1]*x[1][i]+gain[0]*x[2][i];
        new_target[i] = target[i] - tmp;
        err+=new_target[i]*new_target[i];
    }
#endif

    return err;
}
Пример #24
0
void open_loop_nbest_pitch(spx_word16_t* sw, int start, int end, int len, int* pitch, spx_word16_t* gain, int N, char* stack) {
    int i, j, k;
    VARDECL(spx_word32_t * best_score);
    VARDECL(spx_word32_t * best_ener);
    spx_word32_t e0;
    VARDECL(spx_word32_t * corr);
#ifdef FIXED_POINT
    /* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16)
       arrays for (normalized) 16-bit values */
    VARDECL(spx_word16_t * corr16);
    VARDECL(spx_word16_t * ener16);
    spx_word32_t* energy;
    int cshift = 0, eshift = 0;
    int scaledown = 0;
    ALLOC(corr16, end - start + 1, spx_word16_t);
    ALLOC(ener16, end - start + 1, spx_word16_t);
    ALLOC(corr, end - start + 1, spx_word32_t);
    energy = corr;
#else
    /* In floating-point, we need to float arrays and no normalized copies */
    VARDECL(spx_word32_t * energy);
    spx_word16_t* corr16;
    spx_word16_t* ener16;
    ALLOC(energy, end - start + 2, spx_word32_t);
    ALLOC(corr, end - start + 1, spx_word32_t);
    corr16 = corr;
    ener16 = energy;
#endif

    ALLOC(best_score, N, spx_word32_t);
    ALLOC(best_ener, N, spx_word32_t);
    for (i = 0; i < N; i++) {
        best_score[i] = -1;
        best_ener[i] = 0;
        pitch[i] = start;
    }

#ifdef FIXED_POINT
    for (i = -end; i < len; i++) {
        if (ABS16(sw[i]) > 16383) {
            scaledown = 1;
            break;
        }
    }
    /* If the weighted input is close to saturation, then we scale it down */
    if (scaledown) {
        for (i = -end; i < len; i++) {
            sw[i] = SHR16(sw[i], 1);
        }
    }
#endif
    energy[0] = inner_prod(sw - start, sw - start, len);
    e0 = inner_prod(sw, sw, len);
    for (i = start; i < end; i++) {
        /* Update energy for next pitch*/
        energy[i - start + 1] = SUB32(ADD32(energy[i - start], SHR32(MULT16_16(sw[-i - 1], sw[-i - 1]), 6)), SHR32(MULT16_16(sw[-i + len - 1], sw[-i + len - 1]), 6));
        if (energy[i - start + 1] < 0)
            energy[i - start + 1] = 0;
    }

#ifdef FIXED_POINT
    eshift = normalize16(energy, ener16, 32766, end - start + 1);
#endif

    /* In fixed-point, this actually overrites the energy array (aliased to corr) */
    pitch_xcorr(sw, sw - end, corr, len, end - start + 1, stack);

#ifdef FIXED_POINT
    /* Normalize to 180 so we can square it and it still fits in 16 bits */
    cshift = normalize16(corr, corr16, 180, end - start + 1);
    /* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */
    if (scaledown) {
        for (i = -end; i < len; i++) {
            sw[i] = SHL16(sw[i], 1);
        }
    }
#endif

    /* Search for the best pitch prediction gain */
    for (i = start; i <= end; i++) {
        spx_word16_t tmp = MULT16_16_16(corr16[i - start], corr16[i - start]);
        /* Instead of dividing the tmp by the energy, we multiply on the other side */
        if (MULT16_16(tmp, best_ener[N - 1]) > MULT16_16(best_score[N - 1], ADD16(1, ener16[i - start]))) {
            /* We can safely put it last and then check */
            best_score[N - 1] = tmp;
            best_ener[N - 1] = ener16[i - start] + 1;
            pitch[N - 1] = i;
            /* Check if it comes in front of others */
            for (j = 0; j < N - 1; j++) {
                if (MULT16_16(tmp, best_ener[j]) > MULT16_16(best_score[j], ADD16(1, ener16[i - start]))) {
                    for (k = N - 1; k > j; k--) {
                        best_score[k] = best_score[k - 1];
                        best_ener[k] = best_ener[k - 1];
                        pitch[k] = pitch[k - 1];
                    }
                    best_score[j] = tmp;
                    best_ener[j] = ener16[i - start] + 1;
                    pitch[j] = i;
                    break;
                }
            }
        }
    }

    /* Compute open-loop gain if necessary */
    if (gain) {
        for (j = 0; j < N; j++) {
            spx_word16_t g;
            i = pitch[j];
            g = DIV32(SHL32(EXTEND32(corr16[i - start]), cshift), 10 + SHR32(MULT16_16(spx_sqrt(e0), spx_sqrt(SHL32(EXTEND32(ener16[i - start]), eshift))), 6));
            /* FIXME: g = max(g,corr/energy) */
            if (g < 0)
                g = 0;
            gain[j] = g;
        }
    }


}
Пример #25
0
void lsp_to_lpc(const spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/*  float *freq 	array of LSP frequencies in the x domain	*/
/*  float *ak 		array of LPC coefficients 			*/
/*  int lpcrdr  	order of LPC coefficients 			*/
{
    int i,j;
    spx_word32_t xout1,xout2,xin;
    spx_word32_t mult, a;
    VARDECL(spx_word16_t *freqn);
    VARDECL(spx_word32_t **xp);
    VARDECL(spx_word32_t *xpmem);
    VARDECL(spx_word32_t **xq);
    VARDECL(spx_word32_t *xqmem);
    int m = lpcrdr>>1;

    /* 
    
       Reconstruct P(z) and Q(z) by cascading second order polynomials
       in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
       In the time domain this is:

       y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
    
       This is what the ALLOCS below are trying to do:

         int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
         int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP

       These matrices store the output of each stage on each row.  The
       final (m-th) row has the output of the final (m-th) cascaded
       2nd order filter.  The first row is the impulse input to the
       system (not written as it is known).

       The version below takes advantage of the fact that a lot of the
       outputs are zero or known, for example if we put an inpulse
       into the first section the "clock" it 10 times only the first 3
       outputs samples are non-zero (it's an FIR filter).
    */

    ALLOC(xp, (m+1), spx_word32_t*);
    ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);

    ALLOC(xq, (m+1), spx_word32_t*);
    ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
    
    for(i=0; i<=m; i++) {
      xp[i] = xpmem + i*(lpcrdr+1+2);
      xq[i] = xqmem + i*(lpcrdr+1+2);
    }

    /* work out 2cos terms in Q14 */

    ALLOC(freqn, lpcrdr, spx_word16_t);
    for (i=0;i<lpcrdr;i++) 
       freqn[i] = ANGLE2X(freq[i]);

    #define QIMP  21   /* scaling for impulse */

    xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
   
    /* first col and last non-zero values of each row are trivial */
    
    for(i=0;i<=m;i++) {
     xp[i][1] = 0;
     xp[i][2] = xin;
     xp[i][2+2*i] = xin;
     xq[i][1] = 0;
     xq[i][2] = xin;
     xq[i][2+2*i] = xin;
    }

    /* 2nd row (first output row) is trivial */

    xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
    xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);

    xout1 = xout2 = 0;

    /* now generate remaining rows */

    for(i=1;i<m;i++) {

      for(j=1;j<2*(i+1)-1;j++) {
	mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
	xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
	mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
	xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
      }

      /* for last col xp[i][j+2] = xq[i][j+2] = 0 */

      mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
      xp[i+1][j+2] = SUB32(xp[i][j], mult);
      mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
      xq[i+1][j+2] = SUB32(xq[i][j], mult);
    }

    /* process last row to extra a{k} */

    for(j=1;j<=lpcrdr;j++) {
      int shift = QIMP-13;

      /* final filter sections */
      a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift); 
      xout1 = xp[m][j+2];
      xout2 = xq[m][j+2];
      
      /* hard limit ak's to +/- 32767 */

      if (a < -32767) a = -32767;
      if (a > 32767) a = 32767;
      ak[j-1] = (short)a;
     
    }

}
Пример #26
0
static void split_cb_search_shape_sign_N1(
spx_sig_t target[],			/* target vector */
spx_coef_t ak[],			/* LPCs for this subframe */
spx_coef_t awk1[],			/* Weighted LPCs for this subframe */
spx_coef_t awk2[],			/* Weighted LPCs for this subframe */
const void *par,                      /* Codebook/search parameters*/
int   p,                        /* number of LPC coeffs */
int   nsf,                      /* number of samples in subframe */
spx_sig_t *exc,
spx_word16_t *r,
SpeexBits *bits,
char *stack,
int   complexity,
int   update_target
)
{
   int i,j,m,n,q;
   VARDECL(spx_word16_t *resp);
#ifdef _USE_SSE
   VARDECL(__m128 *resp2);
   VARDECL(__m128 *E);
#else
   spx_word16_t *resp2;
   VARDECL(spx_word32_t *E);
#endif
   VARDECL(spx_word16_t *t);
   VARDECL(spx_sig_t *e);
   const signed char *shape_cb;
   int shape_cb_size, subvect_size, nb_subvect;
   const split_cb_params *params;
   int N=2;
   int best_index;
   spx_word32_t best_dist;
   int have_sign;
   N=complexity;
   if (N>10)
      N=10;
   if (N<1)
      N=1;
   
   params = (const split_cb_params *) par;
   subvect_size = params->subvect_size;
   nb_subvect = params->nb_subvect;
   shape_cb_size = 1<<params->shape_bits;
   shape_cb = params->shape_cb;
   have_sign = params->have_sign;
   ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
#ifdef _USE_SSE
   ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
   ALLOC(E, shape_cb_size>>2, __m128);
#else
   resp2 = resp;
   ALLOC(E, shape_cb_size, spx_word32_t);
#endif
   ALLOC(t, nsf, spx_word16_t);
   ALLOC(e, nsf, spx_sig_t);
   
   /* FIXME: make that adaptive? */
   for (i=0;i<nsf;i++)
      t[i]=EXTRACT16(PSHR32(target[i],6));

   compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);

   for (i=0;i<nb_subvect;i++)
   {
      spx_word16_t *x=t+subvect_size*i;
      /*Find new n-best based on previous n-best j*/
      if (have_sign)
         vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
      else
         vq_nbest(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
      
      speex_bits_pack(bits,best_index,params->shape_bits+have_sign);
      
      {
         int rind;
         spx_word16_t *res;
         spx_word16_t sign=1;
         rind = best_index;
         if (rind>=shape_cb_size)
         {
            sign=-1;
            rind-=shape_cb_size;
         }
         res = resp+rind*subvect_size;
         if (sign>0)
            for (m=0;m<subvect_size;m++)
               t[subvect_size*i+m] = SUB16(t[subvect_size*i+m], res[m]);
         else
            for (m=0;m<subvect_size;m++)
               t[subvect_size*i+m] = ADD16(t[subvect_size*i+m], res[m]);

#ifdef FIXED_POINT
         if (sign)
         {
            for (j=0;j<subvect_size;j++)
               e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
         } else {
            for (j=0;j<subvect_size;j++)
               e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
         }
#else
         for (j=0;j<subvect_size;j++)
            e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
#endif
      
      }
            
      for (m=0;m<subvect_size;m++)
      {
         spx_word16_t g;
         int rind;
         spx_word16_t sign=1;
         rind = best_index;
         if (rind>=shape_cb_size)
         {
            sign=-1;
            rind-=shape_cb_size;
         }
         
         q=subvect_size-m;
#ifdef FIXED_POINT
         g=sign*shape_cb[rind*subvect_size+m];
         for (n=subvect_size*(i+1);n<nsf;n++,q++)
            t[n] = SUB32(t[n],MULT16_16_Q11_32(g,r[q]));
#else
         g=sign*0.03125*shape_cb[rind*subvect_size+m];
         for (n=subvect_size*(i+1);n<nsf;n++,q++)
            t[n] = SUB32(t[n],g*r[q]);
#endif
      }
   }

   /* Update excitation */
   /* FIXME: We could update the excitation directly above */
   for (j=0;j<nsf;j++)
      exc[j]=ADD32(exc[j],e[j]);
   
   /* Update target: only update target if necessary */
   if (update_target)
   {
      VARDECL(spx_sig_t *r2);
      ALLOC(r2, nsf, spx_sig_t);
      syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
      for (j=0;j<nsf;j++)
         target[j]=SUB32(target[j],r2[j]);
   }
}
Пример #27
0
void autoCorrelation2LP(word32_t autoCorrelationCoefficients[], word16_t LPCoefficientsQ12[], word32_t reflectionCoefficients[], word32_t *residualEnergy) {
	/*********************************************************************************/
	/* Compute the LP Coefficient using Levinson-Durbin algo spec 3.2.2              */
	/*********************************************************************************/
	/* start a iteration i=2, init values as after iteration i=1 :                   */
	/*        a[0] = 1                                                               */
	/*        a[1] = -r1/r0                                                          */
	/*        E = r0(1 - a[1]^2)                                                     */
	/*                                                                               */
	/*  iterations i = 2..10                                                         */
	/*       sum = r[i] + ∑ a[j]*r[i-j] with j = 1..i-1 (a[0] is always 1)           */
	/*       a[i] = -sum/E                                                           */
	/*       iterations j = 1..i-1                                                   */
	/*            a[j] += a[i]*a{i-1}[i-j] use a{i-1}: from previous iteration       */
	/*       E *=(1-a[i]^2)                                                          */
	/*                                                                               */
	/*  r in Q31 (normalised) stored in array autoCorrelationCoefficients            */
	/*  E in Q31 (can't be > 1)                                                      */
	/*  sum in Q27 (sum can't be > 1 but intermediate accumulation can)              */
	/*  a in Q4.27 with full range possible                                          */
	/*      Note: during iteration, current a[i] is in Q31 (can't be >1) and is      */
	/*            set to Q27 at the end of current iteration                         */
	/*                                                                               */
	/*********************************************************************************/
	word32_t previousIterationLPCoefficients[NB_LSP_COEFF+1]; /* to compute a[]*/
	word32_t LPCoefficients[NB_LSP_COEFF+1]; /* in Q4.27 */

	word32_t E = 0; /* in Q31 */
	word32_t sum = 0; /* in Q27 */
	int i,j;

	/* init */
	LPCoefficients[0] = ONE_IN_Q27;
	LPCoefficients[1] = -DIV32_32_Q27(autoCorrelationCoefficients[1], autoCorrelationCoefficients[0]); /* result in Q27(but<1) */
	reflectionCoefficients[0] = SHL(LPCoefficients[1],4); /* k[0] is -r1/r0 in Q31 */
	/* E = r0(1 - a[1]^2) in Q31 */
	E = MULT32_32_Q31(autoCorrelationCoefficients[0], SUB32(ONE_IN_Q31, MULT32_32_Q23(LPCoefficients[1], LPCoefficients[1]))); /* LPCoefficient[1] is in Q27, using a Q23 operation will result in a Q31 variable */
	
	for (i=2; i<NB_LSP_COEFF+1; i++) {
		/* update the previousIterationLPCoefficients needed for this one */
		for (j=1; j<i; j++) {
			previousIterationLPCoefficients[j] = LPCoefficients[j];
		}

		/* sum = r[i] + ∑ a[j]*r[i-j] with j = 1..i-1 (a[0] is always 1)           */
		sum = 0;
		for (j=1; j<i; j++) {
			sum = MAC32_32_Q31(sum, LPCoefficients[j], autoCorrelationCoefficients[i-j]);/* LPCoefficients in Q27, autoCorrelation in Q31 -> result in Q27 -> sum in Q27 */
		}
		sum = ADD32(SHL(sum, 4), autoCorrelationCoefficients[i]); /* set sum in Q31 and add r[0] */
		
		/* a[i] = -sum/E                                                           */
		LPCoefficients[i] = -DIV32_32_Q31(sum,E); /* LPCoefficient of current iteration is in Q31 for now, it will be set to Q27 at the end of this iteration */
		reflectionCoefficients[i-1] = LPCoefficients[i]; /* k[1] is needed by VAD others by RFC3389 RTP payload for Comfort Noise spectral information encoding */

		/* iterations j = 1..i-1                                                   */
		/*      a[j] += a[i]*a[i-j]                                                */
		for (j=1; j<i; j++) {
			LPCoefficients[j] = MAC32_32_Q31(LPCoefficients[j], LPCoefficients[i], previousIterationLPCoefficients[i-j]); /*LPCoefficients in Q27 except for LPCoefficients[i] in Q31 */
		}
		/* E *=(1-a[i]^2)                                                          */
		E = MULT32_32_Q31(E, SUB32(ONE_IN_Q31, MULT32_32_Q31(LPCoefficients[i], LPCoefficients[i]))); /* all in Q31 */
		/* set LPCoefficients[i] from Q31 to Q27 */
		LPCoefficients[i] = SHR(LPCoefficients[i], 4);
	}
	*residualEnergy = E;

	/* convert with rounding the LP Coefficients form Q27 to Q12, ignore first coefficient which is always 1 */
	for (i=0; i<NB_LSP_COEFF; i++) {
		LPCoefficientsQ12[i] = (word16_t)SATURATE(PSHR(LPCoefficients[i+1], 15), MAXINT16);
	}
}
Пример #28
0
static int quant_coarse_energy_impl(const CELTMode *m, int start, int end,
      const opus_val16 *eBands, opus_val16 *oldEBands,
      opus_int32 budget, opus_int32 tell,
      const unsigned char *prob_model, opus_val16 *error, ec_enc *enc,
      int C, int LM, int intra, opus_val16 max_decay)
{
   int i, c;
   int badness = 0;
   opus_val32 prev[2] = {0,0};
   opus_val16 coef;
   opus_val16 beta;

   if (tell+3 <= budget)
      ec_enc_bit_logp(enc, intra, 3);
   if (intra)
   {
      coef = 0;
      beta = beta_intra;
   } else {
      beta = beta_coef[LM];
      coef = pred_coef[LM];
   }

   /* Encode at a fixed coarse resolution */
   for (i=start;i<end;i++)
   {
      c=0;
      do {
         int bits_left;
         int qi, qi0;
         opus_val32 q;
         opus_val16 x;
         opus_val32 f, tmp;
         opus_val16 oldE;
         opus_val16 decay_bound;
         x = eBands[i+c*m->nbEBands];
         oldE = MAX16(-QCONST16(9.f,DB_SHIFT), oldEBands[i+c*m->nbEBands]);
#ifdef FIXED_POINT
         f = SHL32(EXTEND32(x),7) - PSHR32(MULT16_16(coef,oldE), 8) - prev[c];
         /* Rounding to nearest integer here is really important! */
         qi = (f+QCONST32(.5f,DB_SHIFT+7))>>(DB_SHIFT+7);
         decay_bound = EXTRACT16(MAX32(-QCONST16(28.f,DB_SHIFT),
               SUB32((opus_val32)oldEBands[i+c*m->nbEBands],max_decay)));
#else
         f = x-coef*oldE-prev[c];
         /* Rounding to nearest integer here is really important! */
         qi = (int)floor(.5f+f);
         decay_bound = MAX16(-QCONST16(28.f,DB_SHIFT), oldEBands[i+c*m->nbEBands]) - max_decay;
#endif
         /* Prevent the energy from going down too quickly (e.g. for bands
            that have just one bin) */
         if (qi < 0 && x < decay_bound)
         {
            qi += (int)SHR16(SUB16(decay_bound,x), DB_SHIFT);
            if (qi > 0)
               qi = 0;
         }
         qi0 = qi;
         /* If we don't have enough bits to encode all the energy, just assume
             something safe. */
         tell = ec_tell(enc);
         bits_left = budget-tell-3*C*(end-i);
         if (i!=start && bits_left < 30)
         {
            if (bits_left < 24)
               qi = IMIN(1, qi);
            if (bits_left < 16)
               qi = IMAX(-1, qi);
         }
         if (budget-tell >= 15)
         {
            int pi;
            pi = 2*IMIN(i,20);
            ec_laplace_encode(enc, &qi,
                  prob_model[pi]<<7, prob_model[pi+1]<<6);
         }
         else if(budget-tell >= 2)
         {
            qi = IMAX(-1, IMIN(qi, 1));
            ec_enc_icdf(enc, 2*qi^-(qi<0), small_energy_icdf, 2);
         }
         else if(budget-tell >= 1)
         {
            qi = IMIN(0, qi);
            ec_enc_bit_logp(enc, -qi, 1);
         }
         else
            qi = -1;
         error[i+c*m->nbEBands] = PSHR32(f,7) - SHL16(qi,DB_SHIFT);
         badness += abs(qi0-qi);
         q = (opus_val32)SHL32(EXTEND32(qi),DB_SHIFT);

         tmp = PSHR32(MULT16_16(coef,oldE),8) + prev[c] + SHL32(q,7);
#ifdef FIXED_POINT
         tmp = MAX32(-QCONST32(28.f, DB_SHIFT+7), tmp);
#endif
         oldEBands[i+c*m->nbEBands] = PSHR32(tmp, 7);
         prev[c] = prev[c] + SHL32(q,7) - MULT16_16(beta,PSHR32(q,8));
      } while (++c < C);
   }
   return badness;
}
Пример #29
0
void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
   int i,j,k;
   VARDECL(spx_word32_t *best_score);
   VARDECL(spx_word32_t *best_ener);
   spx_word32_t e0;
   VARDECL(spx_word32_t *corr);
   VARDECL(spx_word32_t *energy);

   ALLOC(best_score, N, spx_word32_t);
   ALLOC(best_ener, N, spx_word32_t);
   ALLOC(corr, end-start+1, spx_word32_t);
   ALLOC(energy, end-start+2, spx_word32_t);

   for (i=0;i<N;i++)
   {
        best_score[i]=-1;
        best_ener[i]=0;
        pitch[i]=start;
   }

   energy[0]=inner_prod(sw-start, sw-start, len);
   e0=inner_prod(sw, sw, len);
   for (i=start;i<end;i++)
   {
      /* Update energy for next pitch*/
      energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
      if (energy[i-start+1] < 0)
         energy[i-start+1] = 0;
   }

   pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);

   /* FIXME: Fixed-point and floating-point code should be merged */
#ifdef FIXED_POINT
   {
      VARDECL(spx_word16_t *corr16);
      VARDECL(spx_word16_t *ener16);
      ALLOC(corr16, end-start+1, spx_word16_t);
      ALLOC(ener16, end-start+1, spx_word16_t);
      /* Normalize to 180 so we can square it and it still fits in 16 bits */
      normalize16(corr, corr16, 180, end-start+1);
      normalize16(energy, ener16, 180, end-start+1);

      for (i=start;i<=end;i++)
      {
         spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
         /* Instead of dividing the tmp by the energy, we multiply on the other side */
         if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
         {
            /* We can safely put it last and then check */
            best_score[N-1]=tmp;
            best_ener[N-1]=ener16[i-start]+1;
            pitch[N-1]=i;
            /* Check if it comes in front of others */
            for (j=0;j<N-1;j++)
            {
               if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
               {
                  for (k=N-1;k>j;k--)
                  {
                     best_score[k]=best_score[k-1];
                     best_ener[k]=best_ener[k-1];
                     pitch[k]=pitch[k-1];
                  }
                  best_score[j]=tmp;
                  best_ener[j]=ener16[i-start]+1;
                  pitch[j]=i;
                  break;
               }
            }
         }
      }
   }
#else
   for (i=start;i<=end;i++)
   {
      float tmp = corr[i-start]*corr[i-start];
      if (tmp*best_ener[N-1]>best_score[N-1]*(1+energy[i-start]))
      {
         for (j=0;j<N;j++)
         {
            if (tmp*best_ener[j]>best_score[j]*(1+energy[i-start]))
            {
               for (k=N-1;k>j;k--)
               {
                  best_score[k]=best_score[k-1];
                  best_ener[k]=best_ener[k-1];
                  pitch[k]=pitch[k-1];
               }
               best_score[j]=tmp;
               best_ener[j]=energy[i-start]+1;
               pitch[j]=i;
               break;
            }
         }
      }
   }
#endif

   /* Compute open-loop gain */
   if (gain)
   {
       for (j=0;j<N;j++)
       {
          spx_word16_t g;
          i=pitch[j];
          g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
          /* FIXME: g = max(g,corr/energy) */
                   if (g<0)
                   g = 0;
             gain[j]=g;
       }
   }
}
Пример #30
0
int LP2LSPConversion(word16_t LPCoefficients[], word16_t LSPCoefficients[])
{
    uint8_t i;

    /*** Compute the polynomials coefficients according to spec 3.2.3 eq15 ***/
    word32_t f1[6];
    word32_t f2[6]; /* coefficients for polynomials F1 anf F2 in Q12 for computation, then converted in Q15 for the Chebyshev Polynomial function */
    f1[0] = ONE_IN_Q12; /* values 0 are not part of the output, they are just used for computation purpose */
    f2[0] = ONE_IN_Q12;
    /* for (i = 0; i< 5; i++) {		*/
    /*   f1[i+1] = a[i+1] + a[10-i] - f1[i];	*/
    /*   f2[i+1] = a[i+1] - a[10-i] + f2[i];	*/
    /* }					*/
    for (i=0; i<5; i++) {
        f1[i+1] = ADD32(LPCoefficients[i], SUB32(LPCoefficients[9-i], f1[i])); /* note: index on LPCoefficients are -1 respect to spec because the unused value 0 is not stored */
        f2[i+1] = ADD32(f2[i], SUB32(LPCoefficients[i], LPCoefficients[9-i])); /* note: index on LPCoefficients are -1 respect to spec because the unused value 0 is not stored */
    }
    /* convert the coefficients from Q12 to Q15 to be used by the Chebyshev Polynomial function (f1/2[0] aren't used so they are not converted) */
    for (i=1; i<6; i++) {
        f1[i] = SHL(f1[i], 3);
        f2[i] = SHL(f2[i], 3);
    }

    /*** Compute at each step(50 steps for the AnnexA version) the Chebyshev polynomial to find the 10 roots ***/
    /* start using f1 polynomials coefficients and altern with f2 after founding each root (spec 3.2.3 eq13 and eq14) */
    uint8_t numberOfRootFound = 0; /* used to check the final number of roots found and exit the loop on each polynomial computation when we have 10 roots */
    word32_t *polynomialCoefficients = f1; /* start with f1 coefficients */
    word32_t previousCx = ChebyshevPolynomial(cosW0pi[0], polynomialCoefficients); /* compute the first point and store it as the previous value for polynomial */
    word32_t Cx; /* value of Chebyshev Polynomial at current point in Q15 */

    for (i=1; i<NB_COMPUTED_VALUES_CHEBYSHEV_POLYNOMIAL; i++) {
        Cx =  ChebyshevPolynomial(cosW0pi[i], polynomialCoefficients);
        if ((previousCx^Cx)&0x10000000) { /* check signe change by XOR on the value of first bit */
            /* divide 2 times the interval to find a more accurate root */
            uint8_t j;
            word16_t xLow = cosW0pi[i-1];
            word16_t xHigh = cosW0pi[i];
            word16_t xMean;
            for (j=0; j<2; j++) {
                xMean = (word16_t)SHR(ADD32(xLow, xHigh), 1);
                word32_t middleCx = ChebyshevPolynomial(xMean, polynomialCoefficients); /* compute the polynome for the value in the middle of current interval */

                if ((previousCx^middleCx)&0x10000000) { /* check signe change by XOR on the value of first bit */
                    xHigh = xMean;
                    Cx = middleCx; /* used for linear interpolation on root */
                } else {
                    xLow = xMean;
                    previousCx = middleCx;
                }
            }

            /* toggle the polynomial coefficients in use between f1 and f2 */
            if (polynomialCoefficients==f1) {
                polynomialCoefficients = f2;
            } else {
                polynomialCoefficients = f1;
            }

            /* linear interpolation for better root accuracy */
            /* xMean = xLow - (xHigh-xLow)* previousCx/(Cx-previousCx); */
            xMean = (word16_t)SUB32(xLow, MULT16_32_Q15(SUB32(xHigh, xLow), DIV32(SHL(previousCx, 14), SHR(SUB32(Cx, previousCx), 1)))); /* Cx are in Q2.15 so we can shift them left 14 bits, the denominator is shifted righ by 1 so the division result is in Q15 */

            /* recompute previousCx with the new coefficients */
            previousCx = ChebyshevPolynomial(xMean, polynomialCoefficients);

            LSPCoefficients[numberOfRootFound] = xMean;

            numberOfRootFound++;
            if (numberOfRootFound == NB_LSP_COEFF) break; /* exit the for loop as soon as we habe all the LSP*/
        }

    }
    if (numberOfRootFound != NB_LSP_COEFF) return 0; /* we were not able to find the 10 roots */


    return 1;
}