Exemple #1
0
void Lsp_Az(
  Word16 lsp[],    /* (i) Q15 : line spectral frequencies            */
  Word16 a[]       /* (o) Q12 : predictor coefficients (order = 10)  */
)
{
  Word16 i, j;
  Word32 f1[6], f2[6];
  Word32 t0;

  Get_lsp_pol(&lsp[0],f1);
  Get_lsp_pol(&lsp[1],f2);

  for (i = 5; i > 0; i--)
  {
    f1[i] = L_add(f1[i], f1[i-1]);        /* f1[i] += f1[i-1]; */
    f2[i] = L_sub(f2[i], f2[i-1]);        /* f2[i] -= f2[i-1]; */
  }

  a[0] = 4096;
  for (i = 1, j = 10; i <= 5; i++, j--)
  {
    t0   = L_add(f1[i], f2[i]);                 /* f1[i] + f2[i]             */
    a[i] = extract_l( L_shr_r(t0, 13) );        /* from Q24 to Q12 and * 0.5 */

    t0   = L_sub(f1[i], f2[i]);                 /* f1[i] - f2[i]             */
    a[j] = extract_l( L_shr_r(t0, 13) );        /* from Q24 to Q12 and * 0.5 */

  }

  return;
}
Exemple #2
0
Word16 Test_Err(Word16 Lag1, Word16 Lag2,ENC_HANDLE *handle)
{

    int i, i1, i2;
    Word16 zone1, zone2;
    Word32 Acc, Err_max;
    Word16 iTest;

    i2 = Lag2 + ClPitchOrd/2;
    zone2 = mult( (Word16) i2, (Word16) 1092);

    i1 = - SubFrLen + 1 + Lag1 - ClPitchOrd/2;
    if(i1 <= 0) i1 = 1;
    zone1 = mult( (Word16) i1, (Word16) 1092);

    Err_max = -1L;
    for(i=zone2; i>=zone1; i--) {
        Acc = L_sub(handle->CodStat.Err[i], Err_max);
        if(Acc > 0L) {
                Err_max = handle->CodStat.Err[i];
        }
    }
    Acc = L_sub(Err_max, ThreshErr);
    if((Acc > 0L) || (handle->CodStat.SinDet < 0 ) ) {
        iTest = 0;
    }
    else {
        Acc = L_negate(Acc);
        Acc = L_shr(Acc, DEC);
        iTest = extract_l(Acc);
    }

    return(iTest);
}
Exemple #3
0
void update_exc_err(
 Word32 *L_exc_err,
 Word16 gain_pit,      /* (i) pitch gain */
 Word16 T0             /* (i) integer part of pitch delay */
)
 {

    Word16 i, zone1, zone2, n;
    Word32 L_worst, L_temp, L_acc;
    Word16 hi, lo;

    L_worst = -1L;
    n = sub(T0, L_SUBFR);

    if(n < 0) {
        L_Extract(L_exc_err[0], &hi, &lo);
        L_temp = Mpy_32_16(hi, lo, gain_pit);
        L_temp = L_shl(L_temp, 1);
        L_temp = L_add(0x00004000L, L_temp);
        L_acc = L_sub(L_temp, L_worst);
        if(L_acc > 0L) {
                L_worst = L_temp;
        }
        L_Extract(L_temp, &hi, &lo);
        L_temp = Mpy_32_16(hi, lo, gain_pit);
        L_temp = L_shl(L_temp, 1);
        L_temp = L_add(0x00004000L, L_temp);
        L_acc = L_sub(L_temp, L_worst);
        if(L_acc > 0L) {
                L_worst = L_temp;
        }
    }

    else {

        zone1 = tab_zone[n];

        i = sub(T0, 1);
        zone2 = tab_zone[i];

        for(i = zone1; i <= zone2; i++) {
                L_Extract(L_exc_err[i], &hi, &lo);
                L_temp = Mpy_32_16(hi, lo, gain_pit);
                L_temp = L_shl(L_temp, 1);
                L_temp = L_add(0x00004000L, L_temp);
                L_acc = L_sub(L_temp, L_worst);
                if(L_acc > 0L) L_worst = L_temp;
        }
    }

    for(i=3; i>=1; i--) {
        L_exc_err[i] = L_exc_err[i-1];
    }
    L_exc_err[0] = L_worst;

    return;
}
Exemple #4
0
Word32 Div_32(Word32 L_num, Word16 denom_hi, Word16 denom_lo)
{
  Word16 approx, hi, lo, n_hi, n_lo;
  Word32 L_32;


  /* First approximation: 1 / L_denom = 1/denom_hi */

  approx = div_s( (Word16)0x3fff, denom_hi);    /* result in Q14 */
                                                /* Note: 3fff = 0.5 in Q15 */

  /* 1/L_denom = approx * (2.0 - L_denom * approx) */

  L_32 = Mpy_32_16(denom_hi, denom_lo, approx); /* result in Q30 */


  L_32 = L_sub( (Word32)0x7fffffffL, L_32);      /* result in Q30 */

  L_Extract(L_32, &hi, &lo);

  L_32 = Mpy_32_16(hi, lo, approx);             /* = 1/L_denom in Q29 */

  /* L_num * (1/L_denom) */

  L_Extract(L_32, &hi, &lo);
  L_Extract(L_num, &n_hi, &n_lo);
  L_32 = Mpy_32(n_hi, n_lo, hi, lo);            /* result in Q29   */
  L_32 = L_shl(L_32, 2);                        /* From Q29 to Q31 */

  return( L_32 );
}
Exemple #5
0
void Log2(
          Word32 x,         /* (i) input           */
          Word16 *int_comp, /* Q0 integer part     */
          Word16 *frac_comp /* Q15 fractional part */
          )
{
   Word16 exp, idx_man, sub_man, sub_tab;
   Word32 a0;

   if(x <= 0){
      *int_comp = 0;
      *frac_comp = 0;
   }
   else{
      exp = norm_l(x);                                   // normalization
      a0 = L_shl(x, exp);                                // Q30 mantissa, i.e. 1.xxx Q30

      /* use table look-up of man in [1.0, 2.0[ Q30 */
      a0 = L_shr(L_sub(a0, (Word32)0x40000000), 8);      // Q16 index into table - note zero'ing of leading 1
      idx_man = extract_h(a0);                           // Q0 index into table
      sub_man = extract_l(L_shr((a0 & 0xFFFF), 1));      // Q15 fractional sub_man
      a0 = L_deposit_h(tablog[idx_man]);                 // Q31
      sub_tab = sub(tablog[idx_man+1], tablog[idx_man]); // Q15
      a0 = L_mac(a0, sub_man, sub_tab);                  // Q31

      *frac_comp = intround(a0);                             // Q15
      *int_comp = sub(30, exp);                          // Q0
   }

   return;
}
Exemple #6
0
void Lsp_pre_select(
  Word16 rbuf[],              /* (i) Q13 : target vetor             */
  Word16 lspcb1[][M],      /* (i) Q13 : first stage LSP codebook */
  Word16 *cand                /* (o)     : selected code            */
)
{
  Word16 i, j;
  Word16 tmp;                 /* Q13 */
  Word32 L_dmin;              /* Q26 */
  Word32 L_tmp;               /* Q26 */
  Word32 L_temp;

                        /* avoid the worst case. (all over flow) */
  *cand = 0;
  L_dmin = MAX_32;
  for ( i = 0 ; i < NC0 ; i++ ) {
    L_tmp = 0;
    for ( j = 0 ; j < M ; j++ ) {
      tmp = sub(rbuf[j], lspcb1[i][j]);
      L_tmp = L_mac( L_tmp, tmp, tmp );
    }

    L_temp = L_sub(L_tmp,L_dmin);
    if (  L_temp< 0L) {
      L_dmin = L_tmp;
      *cand = i;
    }
  }
  return;
}
Exemple #7
0
long interpolation_cos129( short freq )
{
    short  sin_data,cos_data,count,temp ;
    long  Ltemp,Lresult;


    /* cos(x)=cos(a)+(x-a)sin(a)-pow((a-x),2)*cos(a) */

    count=shr(abs_s(freq ),7 );

    temp=sub( extract_l(L_mult( count,64)) , freq );
    /* (a-x)sin a  */
    /* Scale factor for (a-x): 3217=pi2/64 */

    sin_data=sin129_table [ count];
    cos_data=cos129_table [count];

    Ltemp=L_mpy_ls(L_mult(3217,temp),sin_data);

    /* (a-x) sin(a) - [(a-x)*(a-x)*cos(a)] /2 */
    /* Scale factor for (a-x)*(a-x):  20213=pi2*pi2/64 */

    Ltemp=L_sub(Ltemp,
                L_mpy_ls(L_mult(mult_r(10106,temp),temp),cos_data));

    /* Scaled up by 64/2 times */
    Ltemp=L_shl( Ltemp ,6 );
    Lresult=  L_add(L_deposit_h(cos_data), (Ltemp)) ;

    return(Lresult);
}
Exemple #8
0
Word32 L_divide(Word32 L_num, Word32 L_denom)
{
	Word16 approx;
	Word32 L_div;

	if (L_num < 0 || L_denom < 0 || L_num > L_denom)
	{
		printf("ERROR: Invalid input into L_divide!\n");
		return (0);
	}

	/* First approximation: 1 / L_denom = 1/extract_h(L_denom) */

	approx = divide_s((Word16) 0x3fff, extract_h(L_denom));

	/* 1/L_denom = approx * (2.0 - L_denom * approx) */

	L_div = L_mpy_ls(L_denom, approx);

	L_div = L_sub((Word32) 0x7fffffffL, L_div);

	L_div = L_mpy_ls(L_div, approx);

	/* L_num * (1/L_denom) */

	L_div = L_mpy_ll(L_num, L_div);
	L_div = L_shl(L_div, 2);

	return (L_div);
}
Exemple #9
0
void get_pq_polynomials(
    Word32 *f,     /* Q23 */
    Word16 *lsp)   /* Q15 */
{
    Word16 i, n, hi, lo;
    Word16 index, offset, coslsp, c;
    Word32 a0;

    f[0] = L_mult(2048, 2048);                                        // 1.0 Q23
    for(i = 1; i <= LPCO ; i++)
        f[i]= 0;

    for(n=1; n<=(LPCO>>1); n++) {

        /* cosine mapping */
        index = shr(lsp[2*n-2],9);                                     // Q6
        offset = lsp[2*n-2]&(Word16)0x01ff;                            // Q9
        a0 = L_mult(sub(costable[index+1], costable[index]), offset);  // Q10
        coslsp = add(costable[index], intround(L_shl(a0, 6)));            // Q15 cos((double)PI*lsp[2*n-2])

        c = coslsp;                                                    // Q14 c = 2. * cos((double)PI*lsp[2*n-2])

        for(i = 2*n; i >= 2; i--) {
            L_Extract(f[i-1], &hi, &lo);

            f[i] = L_add(f[i], f[i-2]);                                 // Q23 f[i] += f[i-2]
            a0 = Mpy_32_16(hi, lo, c);                                  // Q22
            f[i] = L_sub(f[i], L_shl(a0,1));                            // Q23 f[i] += f[i-2] - c*f[i-1];
        }
        f[1] = L_msu(f[1], c, 256);                                    // Q23 f[1] -= c;
    }

    return;
}
Exemple #10
0
static void Get_lsp_pol(Word16 *lsp, Word32 *f)
{
  Word16 i,j, hi, lo;
  Word32 t0;

   /* All computation in Q24 */

   *f = L_mult(4096, 2048);             /* f[0] = 1.0;             in Q24  */
   f++;
   *f = L_msu((Word32)0, *lsp, 512);    /* f[1] =  -2.0 * lsp[0];  in Q24  */

   f++;
   lsp += 2;                            /* Advance lsp pointer             */

   for(i=2; i<=5; i++)
   {
     *f = f[-2];

     for(j=1; j<i; j++, f--)
     {
       L_Extract(f[-1] ,&hi, &lo);
       t0 = Mpy_32_16(hi, lo, *lsp);         /* t0 = f[-1] * lsp    */
       t0 = L_shl(t0, 1);
       *f = L_add(*f, f[-2]);                /* *f += f[-2]         */
       *f = L_sub(*f, t0);                   /* *f -= t0            */
     }
     *f   = L_msu(*f, *lsp, 512);            /* *f -= lsp<<9        */
     f   += i;                               /* Advance f pointer   */
     lsp += 2;                               /* Advance lsp pointer */
   }

   return;
}
Exemple #11
0
static void cor_h_x_e(
  Word16 h[],    /* (i) Q12 : impulse response of weighted synthesis filter */
  Word16 x[],    /* (i) Q0  : correlation between target and h[]            */
  Word16 dn[]    /* (o) Q0  : correlation between target and h[]            */
)
{
    Word16 i, j, k;
    Word32 s, y32[L_SUBFR], max, tot, L_tmp;

    /* first keep the result on 32 bits and find absolute maximum */
    tot = 5;
    for (k=0; k<NB_TRACK; k++) {
        max = 0;

        for (i=k; i<L_SUBFR; i+=STEP) {
            s = 0;
            for (j=i; j<L_SUBFR; j++) s = L_mac(s, x[j], h[j-i]);
            y32[i] = s;
            s = L_abs(s);
            L_tmp = L_sub(s, max);
            if (L_tmp > (Word32)0) max = s;
        }
        tot = L_add(tot, L_shr(max, 1));    /* tot += (2.0 x max) / 4.0 */
    }

    /* Find the number of right shifts to do on y32[] so that */
    /* 2.0 x sumation of all max of dn[] in each track not saturate. */
    j = sub(norm_l(tot), 2);     /* multiply tot by 4 */
    for (i=0; i<L_SUBFR; i++) {
        dn[i] = round(L_shl(y32[i], j));
    }
    return;
}
Exemple #12
0
void LTP_flag_update (vadState2 * st, Word16 mode)
{
	Word16 thresh;
	Word16 hi1;
	Word16 lo1;
	Word32 Ltmp;

									test(); test();
	if ((sub(mode, MR475) == 0) || (sub(mode, MR515) == 0))
	{
		thresh = (Word16)(32768.0*0.55);			move16();
	}
	else if (sub(mode, MR102) == 0)
	{
		thresh = (Word16)(32768.0*0.60);			move16();
	}
	else
	{
		thresh = (Word16)(32768.0*0.65);			move16();
	}

	L_Extract (st->L_R0, &hi1, &lo1);
	Ltmp = Mpy_32_16(hi1, lo1, thresh);				test();
	if (L_sub(st->L_Rmax, Ltmp) > 0)
	{
		st->LTP_flag = TRUE;					move16();
	}
	else
	{
		st->LTP_flag = FALSE;					move16();
	}

	return;
}
Exemple #13
0
static Word16 Lag_max( /* output: lag found                                  */
  Word16 signal[],     /* input : signal used to compute the open loop pitch */
  Word16 L_frame,      /* input : length of frame to compute pitch           */
  Word16 lag_max,      /* input : maximum lag                                */
  Word16 lag_min,      /* input : minimum lag                                */
  Word16 *cor_max)     /* output: normalized correlation of selected lag     */
{
  Word16  i, j;
  Word16  *p, *p1;
  Word32  max, t0, L_temp;
  Word16  max_h, max_l, ener_h, ener_l;
  Word16  p_max;

  max = MIN_32;

   /* initialization used only to suppress Microsoft Visual C++  warnings */

  p_max = lag_max;

  for (i = lag_max; i >= lag_min; i--)
  {
    p  = signal;
    p1 = &signal[-i];
    t0 = 0;

    for (j=0; j<L_frame; j++, p++, p1++)
      t0 = L_mac(t0, *p, *p1);

    L_temp = L_sub(t0,max);
    if (L_temp >= 0L)
    {
      max    = t0;
      p_max = i;
    }
  }

  /* compute energy */

  t0 = 0;
  p = &signal[-p_max];
  for(i=0; i<L_frame; i++, p++)
    t0 = L_mac(t0, *p, *p);

  /* 1/sqrt(energy),    result in Q30 */

  t0 = Inv_sqrt(t0);

  /* max = max/sqrt(energy)                   */
  /* This result will always be on 16 bits !! */

  L_Extract(max, &max_h, &max_l);
  L_Extract(t0, &ener_h, &ener_l);

  t0 = Mpy_32(max_h, max_l, ener_h, ener_l);
  *cor_max = extract_l(t0);

  return(p_max);
}
Exemple #14
0
void Cor_h_X(
    Word16 h[],        /* (i) Q12 :Impulse response of filters      */
    Word16 X[],        /* (i)     :Target vector                    */
    Word16 D[]         /* (o)     :Correlations between h[] and D[] */
    /*          Normalized to 13 bits            */
)
{
    Word16 i, j;
    Word32 s, max, L_temp;
    Word32 y32[L_SUBFR];

    /* first keep the result on 32 bits and find absolute maximum */

    max = 0;

    for (i = 0; i < L_SUBFR; i++)
    {
        s = 0;
        for (j = i; j <  L_SUBFR; j++)
            s = L_mac(s, X[j], h[j-i]);

        y32[i] = s;

        s = L_abs(s);
        L_temp =L_sub(s,max);
        if(L_temp>0L) {
            max = s;
        }
    }

    /* Find the number of right shifts to do on y32[]  */
    /* so that maximum is on 13 bits                   */

    j = norm_l(max);
    if( sub(j,16) > 0) {
        j = 16;
    }

    j = sub(18, j);

    if(j>=0)
    {
        for(i=0; i<L_SUBFR; i++) {
            D[i] = extract_l( L_shr(y32[i], j) );
        }
    }
    else
    {
        Word16 pj = abs_s(j);
        for(i=0; i<L_SUBFR; i++) {
            D[i] = extract_l( L_shr(y32[i], pj) );
        }
    }

    return;

}
Exemple #15
0
/*----------------------------------------------------------------------------
 *  select_ltp : selects best of (gain1, gain2)
 *  with gain1 = num1 * 2** sh_num1 / den1 * 2** sh_den1
 *  and  gain2 = num2 * 2** sh_num2 / den2 * 2** sh_den2
 *----------------------------------------------------------------------------
 */
static Word16 select_ltp(  /* output : 1 = 1st gain, 2 = 2nd gain */
    Word16 num1,       /* input : numerator of gain1 */
    Word16 den1,       /* input : denominator of gain1 */
    Word16 sh_num1,    /* input : just. factor for num1 */
    Word16 sh_den1,    /* input : just. factor for den1 */
    Word16 num2,       /* input : numerator of gain2 */
    Word16 den2,       /* input : denominator of gain2 */
    Word16 sh_num2,    /* input : just. factor for num2 */
    Word16 sh_den2)    /* input : just. factor for den2 */
{
    Word32 L_temp1, L_temp2;
    Word16 temp1, temp2;
    Word16 hi, lo;
    Word32 L_temp;

    if(den2 == 0) {

        return(1);
    }

    /* compares criteria = num**2/den */
    L_temp1 = L_mult(num1, num1);
    L_Extract(L_temp1, &hi, &lo);
    L_temp1 = Mpy_32_16(hi, lo, den2);

    L_temp2 = L_mult(num2, num2);
    L_Extract(L_temp2, &hi, &lo);
    L_temp2 = Mpy_32_16(hi, lo, den1);

    /* temp1 = sh_den2 + 2 * sh_num1 */
    temp1 = shl(sh_num1, 1);
    temp1 = add(temp1, sh_den2);
    /* temp2 = sh_den1 + 2 * sh_num2; */
    temp2 = shl(sh_num2, 1);
    temp2 = add(temp2, sh_den1);

    if(sub(temp2 ,temp1)>0) {
        temp2 = sub(temp2, temp1);
        L_temp1 = L_shr(L_temp1, temp2);    /* temp2 > 0 */
    }
    else {
        if(sub(temp1 ,temp2) >0) {
            temp1 = sub(temp1, temp2);
            L_temp2 = L_shr(L_temp2, temp1);    /* temp1 > 0 */
        }
    }

    L_temp = L_sub(L_temp2,L_temp1);
    if(L_temp>0L) {

        return(2);
    }
    else {

        return(1);
    }
}
Exemple #16
0
void Lsp_stability(
  Word16 buf[]       /* (i/o) Q13 : quantized LSP parameters      */
)
{
  Word16 j;
  Word16 tmp;
  Word32 L_diff;
  Word32 L_acc, L_accb;

  for(j=0; j<M-1; j++) {
    L_acc = L_deposit_l( buf[j+1] );
    L_accb = L_deposit_l( buf[j] );
    L_diff = L_sub( L_acc, L_accb );

    if( L_diff < 0L ) {
      /* exchange buf[j]<->buf[j+1] */
      tmp      = buf[j+1];
      buf[j+1] = buf[j];
      buf[j]   = tmp;
    }
  }


  if( sub(buf[0], L_LIMIT) <0 ) {
    buf[0] = L_LIMIT;
    printf("lsp_stability warning Low \n");
  }
  for(j=0; j<M-1; j++) {
    L_acc = L_deposit_l( buf[j+1] );
    L_accb = L_deposit_l( buf[j] );
    L_diff = L_sub( L_acc, L_accb );

    if( L_sub(L_diff, GAP3)<0L ) {
      buf[j+1] = add( buf[j], GAP3 );
    }
  }

  if( sub(buf[M-1],M_LIMIT)>0 ) {
    buf[M-1] = M_LIMIT;
    printf("lsp_stability warning High \n");
  }
  return;
}
Exemple #17
0
static Word16 Vq_subvec (/* o : quantization index,            Q0  */
    Word16 *lsf_r1,      /* i : 1st LSF residual vector,       Q15 */
    Word16 *lsf_r2,      /* i : 2nd LSF residual vector,       Q15 */
    const Word16 *dico,  /* i : quantization codebook,         Q15 */
    Word16 *wf1,         /* i : 1st LSF weighting factors      Q13 */
    Word16 *wf2,         /* i : 2nd LSF weighting factors      Q13 */  
    Word16 dico_size     /* i : size of quantization codebook, Q0  */
)
{
    Word16 index = 0; /* initialization only needed to keep gcc silent */
    Word16 i, temp;
    const Word16 *p_dico;
    Word32 dist_min, dist;

    dist_min = MAX_32;                                  move32 (); 
    p_dico = dico;                                      move16 (); 

    for (i = 0; i < dico_size; i++)
    {
        temp = sub (lsf_r1[0], *p_dico++);
        temp = mult (wf1[0], temp);
        dist = L_mult (temp, temp);

        temp = sub (lsf_r1[1], *p_dico++);
        temp = mult (wf1[1], temp);
        dist = L_mac (dist, temp, temp);

        temp = sub (lsf_r2[0], *p_dico++);
        temp = mult (wf2[0], temp);
        dist = L_mac (dist, temp, temp);

        temp = sub (lsf_r2[1], *p_dico++);
        temp = mult (wf2[1], temp);
        dist = L_mac (dist, temp, temp);

        test (); 
        if (L_sub (dist, dist_min) < (Word32) 0)
        {
            dist_min = dist;                            move32 (); 
            index = i;                                  move16 (); 
        }
    }

    /* Reading the selected vector */

    p_dico = &dico[shl (index, 2)];                     move16 (); 
    lsf_r1[0] = *p_dico++;                              move16 (); 
    lsf_r1[1] = *p_dico++;                              move16 (); 
    lsf_r2[0] = *p_dico++;                              move16 (); 
    lsf_r2[1] = *p_dico++;                              move16 (); 

    return index;

}
Exemple #18
0
Word16 test_err(  /* (o) flag set to 1 if taming is necessary  */
 Word32 *L_exc_err,
 Word16 T0,       /* (i) integer part of pitch delay           */
 Word16 T0_frac   /* (i) fractional part of pitch delay        */
)
 {
    Word16 i, t1, zone1, zone2, flag;
    Word32 L_maxloc, L_acc;

    if(T0_frac > 0) {
        t1 = add(T0, 1);
    }
    else {
        t1 = T0;
    }

    i = sub(t1, (L_SUBFR+L_INTER10));
    if(i < 0) {
        i = 0;
    }
    zone1 = tab_zone[i];

    i = add(t1, (L_INTER10 - 2));
    zone2 = tab_zone[i];

    L_maxloc = -1L;
    flag = 0 ;
    for(i=zone2; i>=zone1; i--) {
        L_acc = L_sub(L_exc_err[i], L_maxloc);
        if(L_acc > 0L) {
                L_maxloc = L_exc_err[i];
        }
    }
    L_acc = L_sub(L_maxloc, L_THRESH_ERR);
    if(L_acc > 0L) {
        flag = 1;
    }

    return(flag);
}
Exemple #19
0
Word16 FNevChebP(
                 Word16 x,       /* (i) Q15: value 	               */
                 Word16 *t_man,  /* (i) Q7: mantissa of coefficients */
                 Word16 *t_exp,  /* (i): exponent fo cofficients     */
                 Word16 nd2)     /* (i): order                       */
{
   Word16 i;
   Word16 x2;
   Word16 b_man[NAB], b_exp[NAB];
   Word16 y;
   Word32 a0;

   x2 = x;                                                           // 2x in Q14
   b_man[0] = t_man[nd2];
   b_exp[0] = t_exp[nd2];                                            // b[0] in Q(7+t_exp)
   a0 = L_mult(x2, b_man[0]);
   a0 = L_shr(a0, sub(b_exp[0], 1));                                 // t*b[0] in Q23
   a0 = L_add(a0, L_shr(L_deposit_h(t_man[nd2-1]), t_exp[nd2-1]));   // c[nd2-1] + t*b[0] in Q23
   b_exp[1] = norm_l(a0);
   b_man[1] = intround(L_shl(a0, b_exp[1]));                            // b[1] = c[nd2-1] + t * b[0]

   for (i=2;i<nd2;i++){
      a0 = L_mult(x2, b_man[i-1]);
      a0 = L_shr(a0, sub(b_exp[i-1], 1));                            // t*b[i-1] in Q23
      a0 = L_add(a0, L_shr(L_deposit_h(t_man[nd2-i]), t_exp[nd2-i]));// c[nd2-i] + t*b[i-1] in Q23
      a0 = L_sub(a0, L_shr(L_deposit_h(b_man[i-2]), b_exp[i-2]));    // c[nd2-i] + t*b[i-1] - b[i-2] in Q23
      b_exp[i] = norm_l(a0);
      b_man[i] = intround(L_shl(a0, b_exp[i]));                         // b[i] = c[nd2-i] - b[i-2] + t * b[i-1]
   }

   a0 = L_mult(x, b_man[nd2-1]);
   a0 = L_shr(a0, b_exp[nd2-1]);                                     // x*b[nd2-1] in Q23
   a0 = L_add(a0, L_shr(L_deposit_h(t_man[0]), t_exp[0]));           // c[0] + x*b[nd2-1] in Q23
   a0 = L_sub(a0, L_shr(L_deposit_h(b_man[nd2-2]), b_exp[nd2-2]));   // c[0] + x*b[nd2-1] - b[nd2-2] in Q23

   y = intround(L_shl(a0, 6));                                          // Q13

   return y;                                                         // Q13
}
Exemple #20
0
void Lsp_last_select(
  Word32 L_tdist[],     /* (i) Q27 : distortion         */
  Word16 *mode_index    /* (o)     : the selected mode  */
)
{
    Word32 L_temp;
  *mode_index = 0;
  L_temp =L_sub(L_tdist[1] ,L_tdist[0]);
  if (  L_temp<0L){
    *mode_index = 1;
  }
  return;
}
Exemple #21
0
static Word16   Sqrt( Word32 Num )
{
  Word16   i  ;
  
  Word16   Rez = (Word16) 0 ;
  Word16   Exp = (Word16) 0x4000 ;
  
  Word32   Acc, L_temp;
  
  for ( i = 0 ; i < 14 ; i ++ ) {
    Acc = L_mult(add(Rez, Exp), add(Rez, Exp) );
    L_temp = L_sub(Num, Acc);
    if(L_temp >= 0L) Rez = add( Rez, Exp);
    Exp = shr( Exp, (Word16) 1 ) ;
  }
  return Rez ;
}
Exemple #22
0
static Word16 Cmp_filt(Word16 *RCoeff, Word16 sh_RCoeff, Word16 *acf,
                                        Word16 alpha, Word16 FracThresh)
{
  Word32 L_temp0, L_temp1;
  Word16 temp1, temp2, sh[2], ind;
  Word16 i;
  Word16 diff, flag;
  extern Flag Overflow;

  sh[0] = 0;
  sh[1] = 0;
  ind = 1;
  flag = 0;
  do {
    Overflow = 0;
    temp1 = shr(RCoeff[0], sh[0]);
    temp2 = shr(acf[0], sh[1]);
    L_temp0 = L_shr(L_mult(temp1, temp2),1);
    for(i=1; i <= M; i++) {
      temp1 = shr(RCoeff[i], sh[0]);
      temp2 = shr(acf[i], sh[1]);
      L_temp0 = L_mac(L_temp0, temp1, temp2);
    }
    if(Overflow != 0) {
      sh[(int)ind] = add(sh[(int)ind], 1);
      ind = sub(1, ind);
    }
    else flag = 1;
  } while (flag == 0);
  
  
  temp1 = mult_r(alpha, FracThresh);
  L_temp1 = L_add(L_deposit_l(temp1), L_deposit_l(alpha));
  temp1 = add(sh_RCoeff, 9);  /* 9 = Lpc_justif. * 2 - 16 + 1 */
  temp2 = add(sh[0], sh[1]);
  temp1 = sub(temp1, temp2);
  L_temp1 = L_shl(L_temp1, temp1);
  
  L_temp0 = L_sub(L_temp0, L_temp1);
  if(L_temp0 > 0L) diff = 1;
  else diff = 0;

  return(diff);
}
Exemple #23
0
void lsp2a(
    Word16 lsp[],    /* (i) Q15 : line spectral pairs                  */
    Word16 a[])      /* (o) Q12 : predictor coefficients (order = 10)  */
{
    Word32 p[LPCO+1], q[LPCO+1]; // Q23
    Word32 a0;
    Word16 i, n;

    get_pq_polynomials(p, lsp);
    get_pq_polynomials(q, lsp+1);

    a[0] = 4096;                  // Q12
    a0 = L_add(p[1], q[1]);       // Q23
    a[1] = intround(L_shl(a0,4));    // Q12 - includes 0.5 factor of a[1] = 0.5*(p[1]+q[1])
    for(i=1, n=2; i<LPCO; i++, n++) {
        a0 = L_add(p[i], p[n]);    // Q23
        a0 = L_add(a0, q[n]);      // Q23
        a0 = L_sub(a0, q[i]);      // Q23
        a[n] = intround(L_shl(a0,4)); // Q12 a[n] = 0.5 * (p[i] + p[n] + q[n] - q[i]);
    }

    return;
}
Exemple #24
0
void Lsp_select_2(
  Word16 rbuf[],              /* (i) Q13 : target vector             */
  Word16 lspcb1[],            /* (i) Q13 : first stage lsp codebook  */
  Word16 wegt[],              /* (i) norm: weighting coef.           */
  Word16 lspcb2[][M],      /* (i) Q13 : second stage lsp codebook */
  Word16 *index               /* (o)     : selected codebook index   */
)
{
  Word16 j, k1;
  Word16 buf[M];           /* Q13 */
  Word32 L_dist;              /* Q26 */
  Word32 L_dmin;              /* Q26 */
  Word16 tmp,tmp2;            /* Q13 */
  Word32 L_temp;

  for ( j = NC ; j < M ; j++ )
    buf[j] = sub(rbuf[j], lspcb1[j]);

                            /* avoid the worst case. (all over flow) */
  *index = 0;
  L_dmin = MAX_32;
  for ( k1 = 0 ; k1 < NC1 ; k1++ ) {
    L_dist = 0;
    for ( j = NC ; j < M ; j++ ) {
      tmp = sub(buf[j], lspcb2[k1][j]);
      tmp2 = mult( wegt[j], tmp );
      L_dist = L_mac( L_dist, tmp2, tmp );
    }

    L_temp = L_sub(L_dist, L_dmin);
    if ( L_temp <0L ) {
      L_dmin = L_dist;
      *index = k1;
    }
  }
  return;
}
Exemple #25
0
/*---------------------------------------------------------------------------*
 * Function  Qua_gain                                                        *
 * ~~~~~~~~~~~~~~~~~~                                                        *
 * Inputs:                                                                   *
 *   code[]     :Innovative codebook.                                        *
 *   g_coeff[]  :Correlations compute for pitch.                             *
 *   L_subfr    :Subframe length.                                            *
 *                                                                           *
 * Outputs:                                                                  *
 *   gain_pit   :Quantized pitch gain.                                       *
 *   gain_cod   :Quantized code gain.                                        *
 *                                                                           *
 * Return:                                                                   *
 *   Index of quantization.                                                  *
 *                                                                           *
 *--------------------------------------------------------------------------*/
Word16 Qua_gain(
   Word16 code[],       /* (i) Q13 :Innovative vector.             */
   Word16 g_coeff[],    /* (i)     :Correlations <xn y1> -2<y1 y1> */
                        /*            <y2,y2>, -2<xn,y2>, 2<y1,y2> */
   Word16 exp_coeff[],  /* (i)     :Q-Format g_coeff[]             */
   Word16 L_subfr,      /* (i)     :Subframe length.               */
   Word16 *gain_pit,    /* (o) Q14 :Pitch gain.                    */
   Word16 *gain_cod,    /* (o) Q1  :Code gain.                     */
   Word16 tameflag      /* (i)     : set to 1 if taming is needed  */
)
{
   Word16  i, j, index1, index2;
   Word16  cand1, cand2;
   Word16  exp, gcode0, exp_gcode0, gcode0_org, e_min ;
   Word16  nume, denom, inv_denom;
   Word16  exp1,exp2,exp_nume,exp_denom,exp_inv_denom,sft,tmp;
   Word16  g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
   Word16  coeff[5], coeff_lsf[5];
   Word16  exp_min[5];
   Word32  L_gbk12;
   Word32  L_tmp, L_dist_min, L_tmp1, L_tmp2, L_acc, L_accb;
   Word16  best_gain[2];

        /* Gain predictor, Past quantized energies = -14.0 in Q10 */

 static Word16 past_qua_en[4] = { -14336, -14336, -14336, -14336 };

  /*---------------------------------------------------*
   *-  energy due to innovation                       -*
   *-  predicted energy                               -*
   *-  predicted codebook gain => gcode0[exp_gcode0]  -*
   *---------------------------------------------------*/

   Gain_predict( past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );

  /*-----------------------------------------------------------------*
   *  pre-selection                                                  *
   *-----------------------------------------------------------------*/
  /*-----------------------------------------------------------------*
   *  calculate best gain                                            *
   *                                                                 *
   *  tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ;           *
   *  best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ;  *
   *  best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ;  *
   *  gbk_presel(best_gain,&cand1,&cand2,gcode0) ;                   *
   *                                                                 *
   *-----------------------------------------------------------------*/

  /*-----------------------------------------------------------------*
   *  tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ;           *
   *-----------------------------------------------------------------*/
   L_tmp1 = L_mult( g_coeff[0], g_coeff[2] );
   exp1   = add( add( exp_coeff[0], exp_coeff[2] ), 1-2 );
   L_tmp2 = L_mult( g_coeff[4], g_coeff[4] );
   exp2   = add( add( exp_coeff[4], exp_coeff[4] ), 1 );

   //if( sub(exp1, exp2)>0 ){
   if( exp1 > exp2 ){
      L_tmp = L_sub( L_shr( L_tmp1, sub(exp1,exp2) ), L_tmp2 );
      exp = exp2;
   }
   else{
      L_tmp = L_sub( L_tmp1, L_shr( L_tmp2, sub(exp2,exp1) ) );
      exp = exp1;
   }
   sft = norm_l( L_tmp );
   denom = extract_h( L_shl(L_tmp, sft) );
   exp_denom = sub( add( exp, sft ), 16 );

   inv_denom = div_s(16384,denom);
   inv_denom = negate( inv_denom );
   exp_inv_denom = sub( 14+15, exp_denom );

  /*-----------------------------------------------------------------*
   *  best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ;  *
   *-----------------------------------------------------------------*/
   L_tmp1 = L_mult( g_coeff[2], g_coeff[1] );
   exp1   = add( exp_coeff[2], exp_coeff[1] );
   L_tmp2 = L_mult( g_coeff[3], g_coeff[4] );
   exp2   = add( add( exp_coeff[3], exp_coeff[4] ), 1 );

   //if( sub(exp1, exp2)>0 ){
   if (exp1 > exp2){
      L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1 )), L_shr( L_tmp2,1 ) );
      exp = sub(exp2,1);
   }
   else{
      L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1 )) );
      exp = sub(exp1,1);
   }
   sft = norm_l( L_tmp );
   nume = extract_h( L_shl(L_tmp, sft) );
   exp_nume = sub( add( exp, sft ), 16 );

   sft = sub( add( exp_nume, exp_inv_denom ), (9+16-1) );
   L_acc = L_shr( L_mult( nume,inv_denom ), sft );
   best_gain[0] = extract_h( L_acc );             /*-- best_gain[0]:Q9 --*/

   if (tameflag == 1){
     //if(sub(best_gain[0], GPCLIP2) > 0) best_gain[0] = GPCLIP2;
     if(best_gain[0] > GPCLIP2) best_gain[0] = GPCLIP2;
   }

  /*-----------------------------------------------------------------*
   *  best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ;  *
   *-----------------------------------------------------------------*/
   L_tmp1 = L_mult( g_coeff[0], g_coeff[3] );
   exp1   = add( exp_coeff[0], exp_coeff[3] ) ;
   L_tmp2 = L_mult( g_coeff[1], g_coeff[4] );
   exp2   = add( add( exp_coeff[1], exp_coeff[4] ), 1 );

   //if( sub(exp1, exp2)>0 ){
   if( exp1 > exp2 ){
      L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1) ), L_shr( L_tmp2,1 ) );
      exp = sub(exp2,1);
      //exp = exp2--;
   }
   else{
      L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1) ) );
      exp = sub(exp1,1);
      //exp = exp1--;
   }
   sft = norm_l( L_tmp );
   nume = extract_h( L_shl(L_tmp, sft) );
   exp_nume = sub( add( exp, sft ), 16 );

   sft = sub( add( exp_nume, exp_inv_denom ), (2+16-1) );
   L_acc = L_shr( L_mult( nume,inv_denom ), sft );
   best_gain[1] = extract_h( L_acc );             /*-- best_gain[1]:Q2 --*/

   /*--- Change Q-format of gcode0 ( Q[exp_gcode0] -> Q4 ) ---*/
   //if( sub(exp_gcode0,4) >= 0 ){
   if (exp_gcode0 >=4) {
      gcode0_org = shr( gcode0, sub(exp_gcode0,4) );
   }
   else{
      L_acc = L_deposit_l( gcode0 );
      L_acc = L_shl( L_acc, sub( (4+16), exp_gcode0 ) );
      gcode0_org = extract_h( L_acc );              /*-- gcode0_org:Q4 --*/
   }

  /*----------------------------------------------*
   *   - presearch for gain codebook -            *
   *----------------------------------------------*/

   Gbk_presel(best_gain, &cand1, &cand2, gcode0_org );

/*---------------------------------------------------------------------------*
 *                                                                           *
 * Find the best quantizer.                                                  *
 *                                                                           *
 *  dist_min = MAX_32;                                                       *
 *  for ( i=0 ; i<NCAN1 ; i++ ){                                             *
 *    for ( j=0 ; j<NCAN2 ; j++ ){                                           *
 *      g_pitch = gbk1[cand1+i][0] + gbk2[cand2+j][0];                       *
 *      g_code = gcode0 * (gbk1[cand1+i][1] + gbk2[cand2+j][1]);             *
 *      dist = g_pitch*g_pitch * coeff[0]                                    *
 *           + g_pitch         * coeff[1]                                    *
 *           + g_code*g_code   * coeff[2]                                    *
 *           + g_code          * coeff[3]                                    *
 *           + g_pitch*g_code  * coeff[4] ;                                  *
 *                                                                           *
 *      if (dist < dist_min){                                                *
 *        dist_min = dist;                                                   *
 *        indice1 = cand1 + i ;                                              *
 *        indice2 = cand2 + j ;                                              *
 *      }                                                                    *
 *    }                                                                      *
 *  }                                                                        *
 *                                                                           *
 * g_pitch         = Q13                                                     *
 * g_pitch*g_pitch = Q11:(13+13+1-16)                                        *
 * g_code          = Q[exp_gcode0-3]:(exp_gcode0+(13-1)+1-16)                *
 * g_code*g_code   = Q[2*exp_gcode0-21]:(exp_gcode0-3+exp_gcode0-3+1-16)     *
 * g_pitch*g_code  = Q[exp_gcode0-5]:(13+exp_gcode0-3+1-16)                  *
 *                                                                           *
 * term 0: g_pitch*g_pitch*coeff[0] ;exp_min0 = 13             +exp_coeff[0] *
 * term 1: g_pitch        *coeff[1] ;exp_min1 = 14             +exp_coeff[1] *
 * term 2: g_code*g_code  *coeff[2] ;exp_min2 = 2*exp_gcode0-21+exp_coeff[2] *
 * term 3: g_code         *coeff[3] ;exp_min3 = exp_gcode0  - 3+exp_coeff[3] *
 * term 4: g_pitch*g_code *coeff[4] ;exp_min4 = exp_gcode0  - 4+exp_coeff[4] *
 *                                                                           *
 *---------------------------------------------------------------------------*/

   exp_min[0] = add( exp_coeff[0], 13 );
   exp_min[1] = add( exp_coeff[1], 14 );
   exp_min[2] = add( exp_coeff[2], sub( shl( exp_gcode0, 1 ), 21 ) );
   exp_min[3] = add( exp_coeff[3], sub( exp_gcode0, 3 ) );
   exp_min[4] = add( exp_coeff[4], sub( exp_gcode0, 4 ) );

   e_min = exp_min[0];
   for(i=1; i<5; i++){
      //if( sub(exp_min[i], e_min) < 0 ){
     if (exp_min[i] < e_min) {
         e_min = exp_min[i];
      }
   }

   /* align coeff[] and save in special 32 bit double precision */

   for(i=0; i<5; i++){
     j = sub( exp_min[i], e_min );
     L_tmp = (Word32)g_coeff[i] << 16;
     L_tmp = L_shr( L_tmp, j );          /* L_tmp:Q[exp_g_coeff[i]+16-j] */
     L_Extract( L_tmp, &coeff[i], &coeff_lsf[i] );          /* DPF */
   }

   /* Codebook search */

   L_dist_min = MAX_32;

   /* initialization used only to suppress Microsoft Visual C++  warnings */
   index1 = cand1;
   index2 = cand2;

if(tameflag == 1){
   for(i=0; i<NCAN1; i++){
      for(j=0; j<NCAN2; j++){
         g_pitch = add( gbk1[cand1+i][0], gbk2[cand2+j][0] );     /* Q14 */
         if(g_pitch < GP0999) {
         L_acc = L_deposit_l( gbk1[cand1+i][1] );
         L_accb = L_deposit_l( gbk2[cand2+j][1] );                /* Q13 */
         L_tmp = L_add( L_acc,L_accb );
         tmp = extract_l( L_shr( L_tmp,1 ) );                     /* Q12 */

         g_code   = mult( gcode0, tmp );         /*  Q[exp_gcode0+12-15] */
         g2_pitch = mult(g_pitch, g_pitch);                       /* Q13 */
         g2_code  = mult(g_code,  g_code);       /* Q[2*exp_gcode0-6-15] */
         g_pit_cod= mult(g_code,  g_pitch);      /* Q[exp_gcode0-3+14-15] */

         L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
         L_tmp += Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch);
         L_tmp += Mpy_32_16(coeff[2], coeff_lsf[2], g2_code);
         L_tmp += Mpy_32_16(coeff[3], coeff_lsf[3], g_code);
         L_tmp += Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod);

         //L_temp = L_sub(L_tmp, L_dist_min);

         //if( L_temp < 0L ){
         if( L_tmp < L_dist_min ){
            L_dist_min = L_tmp;
            index1 = add(cand1,i);
            index2 = add(cand2,j);
         }
        }
      }
   }

}
else{
   for(i=0; i<NCAN1; i++){
      for(j=0; j<NCAN2; j++){
         g_pitch = add( gbk1[cand1+i][0], gbk2[cand2+j][0] );     /* Q14 */
         L_acc = L_deposit_l( gbk1[cand1+i][1] );
         L_accb = L_deposit_l( gbk2[cand2+j][1] );                /* Q13 */
         L_tmp = L_add( L_acc,L_accb );
         tmp = extract_l( L_shr( L_tmp,1 ) );                     /* Q12 */

         g_code   = mult( gcode0, tmp );         /*  Q[exp_gcode0+12-15] */
         g2_pitch = mult(g_pitch, g_pitch);                       /* Q13 */
         g2_code  = mult(g_code,  g_code);       /* Q[2*exp_gcode0-6-15] */
         g_pit_cod= mult(g_code,  g_pitch);      /* Q[exp_gcode0-3+14-15] */

         L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
         //L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
         L_tmp += Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch);
         L_tmp += Mpy_32_16(coeff[2], coeff_lsf[2], g2_code);
         L_tmp += Mpy_32_16(coeff[3], coeff_lsf[3], g_code);
         L_tmp += Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod);

         if( L_tmp < L_dist_min ){
            L_dist_min = L_tmp;
            index1 = add(cand1,i);
            index2 = add(cand2,j);
         }

      }
   }
}
   /* Read the quantized gain */

  /*-----------------------------------------------------------------*
   * *gain_pit = gbk1[indice1][0] + gbk2[indice2][0];                *
   *-----------------------------------------------------------------*/
   *gain_pit = add( gbk1[index1][0], gbk2[index2][0] );      /* Q14 */

  /*-----------------------------------------------------------------*
   * *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0;      *
   *-----------------------------------------------------------------*/
   L_gbk12 = (Word32)gbk1[index1][1] + (Word32)gbk2[index2][1]; /* Q13 */
   tmp = extract_l( L_shr( L_gbk12,1 ) );                     /* Q12 */
   L_acc = L_mult(tmp, gcode0);                /* Q[exp_gcode0+12+1] */

   L_acc = L_shl(L_acc, add( negate(exp_gcode0),(-12-1+1+16) ));
   *gain_cod = extract_h( L_acc );                             /* Q1 */

  /*----------------------------------------------*
   * update table of past quantized energies      *
   *----------------------------------------------*/
   Gain_update( past_qua_en, L_gbk12 );

   return( add( map1[index1]*(Word16)NCODE2, map2[index2] ) );

}
Exemple #26
0
/*---------------------------------------------------------------------------*
 * Function Gbk_presel                                                       *
 * ~~~~~~~~~~~~~~~~~~~                                                       *
 *   - presearch for gain codebook -                                         *
 *---------------------------------------------------------------------------*/
static void Gbk_presel(
   Word16 best_gain[],     /* (i) [0] Q9 : unquantized pitch gain     */
                           /* (i) [1] Q2 : unquantized code gain      */
   Word16 *cand1,          /* (o)    : index of best 1st stage vector */
   Word16 *cand2,          /* (o)    : index of best 2nd stage vector */
   Word16 gcode0           /* (i) Q4 : presearch for gain codebook    */
)
{
   Word16    acc_h;
   Word16    sft_x,sft_y;
   Word32    L_acc,L_preg,L_cfbg,L_tmp,L_tmp_x,L_tmp_y;
   Word32 L_temp;

 /*--------------------------------------------------------------------------*
   x = (best_gain[1]-(coef[0][0]*best_gain[0]+coef[1][1])*gcode0) * inv_coef;
  *--------------------------------------------------------------------------*/
   L_cfbg = L_mult( coef[0][0], best_gain[0] );        /* L_cfbg:Q20 -> !!y */
   L_acc = L_shr( L_coef[1][1], 15 );                  /* L_acc:Q20     */
   L_acc = L_add( L_cfbg , L_acc );
   acc_h = extract_h( L_acc );                         /* acc_h:Q4      */
   L_preg = L_mult( acc_h, gcode0 );                   /* L_preg:Q9     */
   L_acc = L_shl( L_deposit_l( best_gain[1] ), 7 );    /* L_acc:Q9      */
   L_acc = L_sub( L_acc, L_preg );
   acc_h = extract_h( L_shl( L_acc,2 ) );              /* L_acc_h:Q[-5] */
   L_tmp_x = L_mult( acc_h, INV_COEF );                /* L_tmp_x:Q15   */

 /*--------------------------------------------------------------------------*
   y = (coef[1][0]*(-coef[0][1]+best_gain[0]*coef[0][0])*gcode0
                                      -coef[0][0]*best_gain[1]) * inv_coef;
  *--------------------------------------------------------------------------*/
   L_acc = L_shr( L_coef[0][1], 10 );                  /* L_acc:Q20   */
   L_acc = L_sub( L_cfbg, L_acc );                     /* !!x -> L_cfbg:Q20 */
   acc_h = extract_h( L_acc );                         /* acc_h:Q4    */
   acc_h = mult( acc_h, gcode0 );                      /* acc_h:Q[-7] */
   L_tmp = L_mult( acc_h, coef[1][0] );                /* L_tmp:Q10   */

   L_preg = L_mult( coef[0][0], best_gain[1] );        /* L_preg:Q13  */
   L_acc = L_sub( L_tmp, L_shr(L_preg,3) );            /* L_acc:Q10   */

   acc_h = extract_h( L_shl( L_acc,2 ) );              /* acc_h:Q[-4] */
   L_tmp_y = L_mult( acc_h, INV_COEF );                /* L_tmp_y:Q16 */

   sft_y = (14+4+1)-16;         /* (Q[thr1]+Q[gcode0]+1)-Q[L_tmp_y] */
   sft_x = (15+4+1)-15;         /* (Q[thr2]+Q[gcode0]+1)-Q[L_tmp_x] */

   if(gcode0>0){
      /*-- pre select codebook #1 --*/
      *cand1 = 0 ;
      do{
         L_temp = L_sub( L_tmp_y, L_shr(L_mult(thr1[*cand1],gcode0),sft_y));
         if(L_temp >0L  ){
        //(*cand1) =add(*cand1,1);
           *cand1 += 1;
     }
         else               break ;
      //} while(sub((*cand1),(NCODE1-NCAN1))<0) ;
      } while ((*cand1) < (NCODE1-NCAN1));
      /*-- pre select codebook #2 --*/
      *cand2 = 0 ;
      do{
        L_temp = L_sub( L_tmp_x , L_shr(L_mult(thr2[*cand2],gcode0),sft_x));
         if( L_temp >0L) {
        //(*cand2) =add(*cand2,1);
           *cand2 += 1;
     }
         else               break ;
      //} while(sub((*cand2),(NCODE2-NCAN2))<0) ;
      } while((*cand2) < (NCODE2-NCAN2)) ;
   }
   else{
      /*-- pre select codebook #1 --*/
      *cand1 = 0 ;
      do{
        L_temp = L_sub(L_tmp_y ,L_shr(L_mult(thr1[*cand1],gcode0),sft_y));
         if( L_temp <0L){
        //(*cand1) =add(*cand1,1);
           *cand1 += 1;
     }
         else               break ;
      //} while(sub((*cand1),(NCODE1-NCAN1))) ;
      } while (*cand1 != (NCODE1-NCAN1)) ;
      /*-- pre select codebook #2 --*/
      *cand2 = 0 ;
      do{
         L_temp =L_sub(L_tmp_x ,L_shr(L_mult(thr2[*cand2],gcode0),sft_x));
         if( L_temp <0L){
        //(*cand2) =add(*cand2,1);
           *cand2 += 1;
     }
         else               break ;
      //} while(sub( (*cand2),(NCODE2-NCAN2))) ;
      } while(*cand2 != (NCODE2-NCAN2)) ;
   }

   return ;
}
Exemple #27
0
Word16 coarsepitch(
                   Word16  *xw,           /* (i) (normalized) weighted signal */
                   struct BV16_Encoder_State *cstate) /* (i/o) Coder State */
{
   
   Word16   s;    /* Q2 */
   Word16   a, b; 
   Word16   im;
   Word16   maxdev, flag, mpflag;
   Word32   eni, deltae;
   Word32   cc;
   Word16   ah,al, bh, bl;
   Word32   a0, a1, a2, a3;
   Word32   *lp0; 
   Word16   exp, new_exp;  
   Word16   *fp0, *fp1, *fp2, *fp3, *sp; 
   Word16   *fp1_h, *fp1_l, *fp2_h, *fp2_l;
   Word16   cor2max, cor2max_exp;
   Word16   cor2m, cor2m_exp;
   Word16   s0, t0, t1, exp0, exp1, e2, e3;
   Word16   threshold;
   Word16   mplth;      /* Q2 */
   Word16   i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1];
   Word16   cpp;
   Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1];
   Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD];
   Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */
   Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD];
   Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1];
   Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD];
   Word16 energym_man, energym_exp;
   Word16 energymax_man, energymax_exp;
      
   /* Lowpass filter xw() to 800 hz; shift & output into xwd() */
   
   /* AP and AZ filtering and decimation */
   
   fp1_h = tmp_h + DFO;
   fp1_l = tmp_l + DFO;
   
   sp = xw;
   a1 = 1;
   
   
   for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1];
   for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i];
   
   lp0 = lxwd;
   
   for (i=0;i<FRSZD;i++) {
      for (k=0;k<DECF;k++) {
         a0 = L_shr(L_deposit_h(*sp++),10);
         fp2_h = fp1_h-1;
         fp2_l = fp1_l-1;
         for (j=0;j<DFO;j++)  
            a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1]));
         a0 = L_shl(a0, 2);             /* adf Q13 */
         L_Extract(a0, fp1_h++, fp1_l++);
      }
      fp2_h = fp1_h-1;
      fp2_l = fp1_l-1;
      a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]);
      for (j=0;j<DFO;j++)
         a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1]));
      
      *lp0++ = a0;
      a0 = L_abs(a0);
      
      if (a1 < a0) 
         a1 = a0;
   }
   
   /* copy temp buffer to memory */
   fp1_h -= DFO;
   fp1_l -= DFO;
   for (i=0;i<DFO;i++) {
      cstate->dfm_h[2*i+1] = fp1_h[i];
      cstate->dfm_h[2*i] = fp1_l[i];
   }
   
   lp0 = lxwd;
   new_exp = sub(norm_l(a1), 3);       /* headroom to avoid overflow */
   exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */
   
   if (exp < 0) { /* Descending signal level */
      new_exp = cstate->xwd_exp;
      exp = 0;
   }
   
   for (i=0;i<XDOFF;i++) 
      xwd[i] = shr(cstate->xwd[i], exp);
   
   /* fill-in new exponent */
   fp0 = xwd + XDOFF;
   for (i=0;i<FRSZD;i++) 
      fp0[i] = intround(L_shl(lp0[i],new_exp));
   
   /* update signal memory for next frame */
   exp0 = 1;
   for (i=0;i<XDOFF;i++) {
      exp1 = abs_s(xwd[FRSZD+i]);
      
      if (exp1 > exp0) 
         exp0 = exp1;
   }
   exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */
   
   exp = sub(exp0, exp);
   
   if (exp >=0)
   {
      for (i=0;i<XDOFF-FRSZD;i++)  
         cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp);
   }
   else
   {
      exp = -exp;
      if (exp >=15)
         exp = 15;
      for (i=0;i<XDOFF-FRSZD;i++)  
         cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp);
   }
   for (;i<XDOFF;i++) 
      cstate->xwd[i] = shl(xwd[FRSZD+i],exp0);
   
   
   cstate->xwd_exp = add(new_exp, exp0);
   
   /* Compute correlation & energy of prediction basis vector */
   
   /* reset local buffers */
   for (i=0;i<MAXPPD1;i++) 
      cor[i] = energy[i] = 0;
   
   fp0 = xwd+MAXPPD1;
   fp1 = xwd+MAXPPD1-M1;
   a0 = a1 = 0; 
   for (i=0;i<(LXD-MAXPPD1);i++) {
      a0 = L_mac0(a0, *fp1, *fp1);
      a1 = L_mac0(a1, *fp0++, *fp1++);
   }
   cor[M1-1] = a1;
   energy[M1-1] = a0;
   energy_exp[M1-1] = norm_l(energy[M1-1]);
   energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1]));
   s0 = cor2_exp[M1-1] = norm_l(a1);
   t0 = extract_h(L_shl(a1, s0));
   cor2[M1-1] = extract_h(L_mult(t0, t0));
   
   if (a1 < 0) 
      cor2[M1-1] = negate(cor2[M1-1]);
   
   fp2 = xwd+LXD-M1-1;
   fp3 = xwd+MAXPPD1-M1-1;
   for (i=M1;i<M2;i++) {
      fp0 = xwd+MAXPPD1;
      fp1 = xwd+MAXPPD1-i-1;
      a1 = 0;
      for (j=0;j<(LXD-MAXPPD1);j++) 
         a1 = L_mac0(a1,*fp0++,*fp1++); 
      cor[i] = a1;
      a0 = L_msu0(a0, *fp2, *fp2);
      a0 = L_mac0(a0, *fp3, *fp3);
      fp2--; fp3--;
      energy[i] = a0;
      energy_exp[i] = norm_l(energy[i]);
      energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i]));
      s0 = cor2_exp[i] = norm_l(a1);
      t0 = extract_h(L_shl(a1, s0));
      cor2[i] = extract_h(L_mult(t0, t0));
      
      if (a1 < 0) 
         cor2[i] = negate(cor2[i]);
   }
   
   /* Find positive correlation peaks */
   /* Find maximum of cor*cor/energy among positive correlation peaks */ 
   
   npeaks = 0;
   n = MINPPD-1;
   while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) {
      
      if (cor[n]>0) { 
         a0   = L_mult(energy_man[n-1],cor2[n]);
         a1   = L_mult(energy_man[n], cor2[n-1]);
         exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1);
         exp0 = add(exp0, energy_exp[n-1]);
         exp0 = sub(exp0, energy_exp[n]);
         
         if (exp0>=0) 
            a0 = L_shr(a0, exp0);
         else 
            a1 = L_shl(a1, exp0);
         
         if (a0 > a1) { 
            
            a0   = L_mult(energy_man[n+1],cor2[n]);
            a1   = L_mult(energy_man[n], cor2[n+1]);
            exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1);
            exp0 = add(exp0, energy_exp[n+1]);
            exp0 = sub(exp0, energy_exp[n]);
            
            if (exp0>=0) 
               a0 = L_shr(a0, exp0);
            else 
               a1 = L_shl(a1, exp0);
            
            if (a0 > a1) {
               idx[npeaks] = n;
               npeaks++; 
            }
         }
    }
    
    n++;
    
  }
  
  /* Return early if there is no peak or only one peak */
  
  if (npeaks == 0){   /* if there are no positive peak, */
     return MINPPD*DECF; /* return minimum pitch period in decimated domain */
  }
  
  if (npeaks == 1){   /* if there is exactly one peak, */
     return (idx[0]+1)*DECF; /* return the time lag for this single peak */
  }
  
  /* If program proceeds to here, there are 2 or more peaks */
  cor2max=(Word16) 0x8000;
  cor2max_exp= (Word16) 0;
  energymax_man=1;
  energymax_exp=0;
  
  imax=0;
  for (i=0; i < npeaks; i++) {
     
  /* Use quadratic interpolation to find the interpolated cor[] and
     energy[] corresponding to interpolated peak of cor2[]/energy[] */
     /* first calculate coefficients of y(x)=ax^2+bx+c; */
     n=idx[i];
     a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]);
     L_Extract(a0, &ah, &al);
     a0=L_shr(L_sub(cor[n+1],cor[n-1]),1);
     L_Extract(a0, &bh, &bl);
     cc=cor[n];
     
     /* Initialize variables before searching for interpolated peak */
     im=0;
     cor2m_exp = cor2_exp[n];
     cor2m = cor2[n];
     energym_exp = energy_exp[n];
     energym_man = energy_man[n];
     eni=energy[n];
     
     /* Determine which side the interpolated peak falls in, then
     do the search in the appropriate range */
     
     a0   = L_mult(energy_man[n-1],cor2[n+1]);
     a1   = L_mult(energy_man[n+1], cor2[n-1]);
     exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1);
     exp0 = add(exp0, energy_exp[n-1]);
     exp0 = sub(exp0, energy_exp[n+1]);
     
     if (exp0>=0) 
        a0 = L_shr(a0, exp0);
     else 
        a1 = L_shl(a1, exp0);
     
     if (a0 > a1) {  /* if right side */
        
        deltae = L_shr(L_sub(energy[n+1], eni), 2);
        
        for (k = 0; k < HDECF; k++) {
           a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
           eni = L_add(eni, deltae);
           a1 = eni;
           exp0 = norm_l(a0);
           s0 = extract_h(L_shl(a0, exp0));
           s0 = extract_h(L_mult(s0, s0));
           e2 = energym_exp;
           t0 = energym_man;
           a2 = L_mult(t0, s0);
           e3 = norm_l(a1);
           t1 = extract_h(L_shl(a1, e3));
           a3 = L_mult(t1, cor2m);
           exp1 = shl(sub(exp0, cor2m_exp),1);
           exp1 = add(exp1, e2);
           exp1 = sub(exp1, e3);
           
           if (exp1>=0) 
              a2 = L_shr(a2, exp1);
           else 
              a3 = L_shl(a3, exp1);
           
           if (a2 > a3) {
              im = k+1;
              cor2m = s0;
              cor2m_exp = exp0;
              energym_exp = e3;
              energym_man = t1;
           }
        }        
     } else {    /* if interpolated peak is on the left side */
        
        deltae = L_shr(L_sub(energy[n-1], eni), 2);
        for (k = 0; k < HDECF; k++) {
           a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
           eni = L_add(eni, deltae);
           a1=eni;
           
           exp0 = norm_l(a0);
           s0 = extract_h(L_shl(a0, exp0));
           s0 = extract_h(L_mult(s0, s0));
           e2 = energym_exp;
           t0 = energym_man;
           a2 = L_mult(t0, s0);
           e3 = norm_l(a1);
           t1 = extract_h(L_shl(a1, e3));
           a3 = L_mult(t1, cor2m);
           exp1 = shl(sub(exp0, cor2m_exp),1);
           exp1 = add(exp1, e2);
           exp1 = sub(exp1, e3);
           
           if (exp1>=0) 
              a2 = L_shr(a2, exp1);
           else 
              a3 = L_shl(a3, exp1);
           
           if (a2 > a3) {
              im = -k-1;
              cor2m = s0;
              cor2m_exp = exp0;
              energym_exp = e3;
              energym_man = t1;
           }
        }        
     }
     
     /* Search done; assign cor2[] and energy[] corresponding to 
     interpolated peak */ 
     plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */
     cor2i[i]=cor2m;
     cor2i_exp[i]=cor2m_exp;
     /* interpolated energy[] of i-th interpolated peak */
     energyi_exp[i] = energym_exp;
     energyi_man[i] = energym_man;
     
     /* Search for global maximum of interpolated cor2[]/energy[] peak */
     a0 = L_mult(cor2m,energymax_man);
     a1 = L_mult(cor2max, energyi_man[i]);
     exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
     exp0 = add(exp0, energymax_exp);
     exp0 = sub(exp0, energyi_exp[i]);
     
     if (exp0 >=0) 
        a0 = L_shr(a0, exp0);
     else 
        a1 = L_shl(a1, exp0);
     
     if (a0 > a1) {
        imax=i;
        cor2max=cor2m;
        cor2max_exp=cor2m_exp;
        energymax_exp = energyi_exp[i];
        energymax_man = energyi_man[i];
     }
  }
  cpp=plag[imax]; /* first candidate for coarse pitch period */
  mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */
  
  /* Find the largest peak (if there is any) around the last pitch */
  maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */
  im = -1;
  cor2m=(Word16) 0x8000;
  cor2m_exp= (Word16) 0;
  energym_man = 1;
  energym_exp = 0;
  
  for (i=0;i<npeaks;i++) {  /* loop thru the peaks before the largest peak */
     
     if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) {
        a0 = L_mult(cor2i[i],energym_man);
        a1 = L_mult(cor2m, energyi_man[i]);
        exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1);
        exp0 = add(exp0, energym_exp);
        exp0 = sub(exp0, energyi_exp[i]);
        
        if (exp0 >=0) 
           a0 = L_shr(a0, exp0);
        else 
           a1 = L_shl(a1, exp0);
        
        if (a0 > a1) {
           im=i;
           cor2m=cor2i[i];
           cor2m_exp=cor2i_exp[i];
           energym_man = energyi_man[i];
           energym_exp = energyi_exp[i];
        }   
     }
  } /* if there is no peaks around last pitch, then im is still -1 */
  
  
  /* Now see if we should pick any alternatice peak */
  /* first, search first half of pitch range, see if any qualified peak
  has large enough peaks at every multiple of its lag */
  i=0;
  
  while (2*plag[i] < mplth) {
     
     /* Determine the appropriate threshold for this peak */
     
     if (i != im) {  /* if not around last pitch, */
        threshold = TH1;    /* use a higher threshold */
     } else {        /* if around last pitch */
        threshold = TH2;    /* use a lower threshold */
     }
     
     /* If threshold exceeded, test peaks at multiples of this lag */
     a0 = L_mult(cor2i[i],energymax_man);
     t1 = extract_h(L_mult(energyi_man[i], threshold));
     a1 = L_mult(cor2max, t1);
     exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1);
     exp0 = add(exp0, energymax_exp);
     exp0 = sub(exp0, energyi_exp[i]);
     
     if (exp0 >=0) 
        a0 = L_shr(a0, exp0);
     else 
        a1 = L_shl(a1, exp0);
     
     if (a0 > a1) {
        flag=1;  
        j=i+1;
        k=0;
        s=shl(plag[i],1); /* initialize t to twice the current lag */
        
        while (s<=mplth) { /* loop thru all multiple lag <= mplth */
           
           mpflag=0;   /* initialize multiple pitch flag to 0 */
           t0 = mult_r(s,MPDTH); 
           a=sub(s, t0);   /* multiple pitch range lower bound */
           b=add(s, t0);   /* multiple pitch range upper bound */
           while (j < npeaks) { /* loop thru peaks with larger lags */
              
              if (plag[j] > b) { /* if range exceeded, */
                 break;          /* break the innermost while loop */
              }       /* if didn't break, then plag[j] <= b */
              
              if (plag[j] > a) { /* if current peak lag within range, */
                 /* then check if peak value large enough */
                 a0 = L_mult(cor2i[j],energymax_man);
                 if (k<4) 
                    t1 = MPTH[k];
                 else 
                    t1 = MPTH4;
                 t1 = extract_h(L_mult(t1, energyi_man[j]));
                 a1 = L_mult(cor2max, t1);
                 exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1);
                 exp0 = add(exp0, energymax_exp);
                 exp0 = sub(exp0, energyi_exp[j]);
                 
                 if (exp0 >=0) 
                    a0 = L_shr(a0, exp0);
                 else 
                    a1 = L_shl(a1, exp0);
                 
                 if (a0 > a1) {
                    mpflag=1; /* if peak large enough, set mpflag, */
                    break; /* and break the innermost while loop */
                 } 
              }
              j++;
           }
           /* if no qualified peak found at this multiple lag */
           
           if (mpflag == 0) { 
              flag=0;     /* disqualify the lag plag[i] */
              break;      /* and break the while (s<=mplth) loop */
           }
           k++;
           s = add(s, plag[i]); /* update s to the next multiple pitch lag */
           
        }
        /* if there is a qualified peak at every multiple of plag[i], */
        
        if (flag == 1) { 
           cpp = plag[i];   /* then accept this as final pitch */

           return cpp;         /* and return to calling function */
        }
    }       
    i++;
    
    if (i == npeaks)
       break;      /* to avoid out of array bound error */
  }
  
  /* If program proceeds to here, none of the peaks with lags < 0.5*mplth
  qualifies as the final pitch. in this case, check if
  there is any peak large enough around last pitch.  if so, use its
  lag as the final pitch. */
  
  if (im != -1) {   /* if there is at least one peak around last pitch */
     
     if (im == imax) { /* if this peak is also the global maximum, */
        return cpp;   /* return first pitch candidate at global max */
     }
     
     if (im < imax) { /* if lag of this peak < lag of global max, */
        a0 = L_mult(cor2m,energymax_man);
        t1 = extract_h(L_mult(energym_man, LPTH2));
        a1 = L_mult(cor2max, t1);
        exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
        exp0 = add(exp0, energymax_exp);
        exp0 = sub(exp0, energym_exp);
        
        if (exp0 >=0) 
           a0 = L_shr(a0, exp0);
        else 
           a1 = L_shl(a1, exp0);
        
        if (a0 > a1) {
           
           if (plag[im] > HMAXPPD*DECF) {
              cpp=plag[im];
              
              return cpp;
           }
           for (k=2; k<=5;k++) { /* check if current candidate pitch */
              s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */
              t0 = mult_r(s,SMDTH);
              a=sub(s, t0);      /* the time lag of */
              b=add(s, t0);       /* the global maximum peak */
              
              if (plag[im]>a && plag[im]<b) {     /* if so, */
                 cpp=plag[im];       /* accept this peak, */
                 
                 return cpp;         /* and return as pitch */
              }
           }
        }
     } else {           /* if lag of this peak > lag of global max, */
        a0 = L_mult(cor2m,energymax_man);
        t1 = extract_h(L_mult(energym_man, LPTH1));
        a1 = L_mult(cor2max, t1);
        exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
        exp0 = add(exp0, energymax_exp);
        exp0 = sub(exp0, energym_exp);
        
        if (exp0 >=0) 
           a0 = L_shr(a0, exp0);
        else 
           a1 = L_shl(a1, exp0);
        
        if (a0 > a1) {
           cpp = plag[im];  /* if this peak is large enough, */
           
           return cpp;         /* accept its lag */ 
        }
     }
  }
  
  /* If program proceeds to here, we have no choice but to accept the
  lag of the global maximum */
  return cpp;
  
}
Exemple #28
0
void pit_pst_filt(
  Word16 *signal,      /* (i)     : input signal                        */
  Word16 *scal_sig,    /* (i)     : input signal (scaled, divided by 4) */
  Word16 t0_min,       /* (i)     : minimum value in the searched range */
  Word16 t0_max,       /* (i)     : maximum value in the searched range */
  Word16 L_subfr,      /* (i)     : size of filtering                   */
  Word16 *signal_pst   /* (o)     : harmonically postfiltered signal    */
)
{
  Word16 i, j, t0;
  Word16 g0, gain, cmax, en, en0;
  Word16 *p, *p1, *deb_sig;
  Word32 corr, cor_max, ener, ener0, temp;
  Word32 L_temp;

/*---------------------------------------------------------------------------*
 * Compute the correlations for all delays                                   *
 * and select the delay which maximizes the correlation                      *
 *---------------------------------------------------------------------------*/

  deb_sig = &scal_sig[-t0_min];
  cor_max = MIN_32;
  t0 = t0_min;             /* Only to remove warning from some compilers */
  for (i=t0_min; i<=t0_max; i++)
  {
    corr = 0;
    p    = scal_sig;
    p1   = deb_sig;
    for (j=0; j<L_subfr; j++)
       corr = L_mac(corr, *p++, *p1++);

    L_temp = L_sub(corr, cor_max);
    if (L_temp > (Word32)0)
    {
      cor_max = corr;
      t0 = i;
    }
    deb_sig--;
  }

  /* Compute the energy of the signal delayed by t0 */

  ener = 1;
  p = scal_sig - t0;
  for ( i=0; i<L_subfr ;i++, p++)
    ener = L_mac(ener, *p, *p);

  /* Compute the signal energy in the present subframe */

  ener0 = 1;
  p = scal_sig;
  for ( i=0; i<L_subfr; i++, p++)
    ener0 = L_mac(ener0, *p, *p);

  if (cor_max < 0)
  {
    cor_max = 0;
  }

  /* scale "cor_max", "ener" and "ener0" on 16 bits */

  temp = cor_max;
  if (ener > temp)
  {
    temp = ener;
  }
  if (ener0 > temp)
  {
    temp = ener0;
  }
  j = norm_l(temp);
  if(j>0)
  {
  cmax = round(L_shl(cor_max, j));
  en = round(L_shl(ener, j));
  en0 = round(L_shl(ener0, j));
  }
  else
  {
  	Word16 pj = abs_s(j);
  	cmax = round(L_shr(cor_max, pj));	
  	en = round(L_shr(ener, pj));
  	en0 = round(L_shr(ener0, pj));
  }
  /* prediction gain (dB)= -10 log(1-cor_max*cor_max/(ener*ener0)) */

  /* temp = (cor_max * cor_max) - (0.5 * ener * ener0)  */
  temp = L_mult(cmax, cmax);
  temp = L_sub(temp, L_shr(L_mult(en, en0), 1));

  if (temp < (Word32)0)           /* if prediction gain < 3 dB   */
  {                               /* switch off pitch postfilter */
    for (i = 0; i < L_subfr; i++)
      signal_pst[i] = signal[i];
    return;
  }

  if (sub(cmax, en) > 0)      /* if pitch gain > 1 */
  {
    g0 = INV_GAMMAP;
    gain = GAMMAP_2;
  }
  else {
    cmax = shr(mult(cmax, GAMMAP), 1);  /* cmax(Q14) = cmax(Q15) * GAMMAP */
    en = shr(en, 1);          /* Q14 */
    i = add(cmax, en);
    if(i > 0)
    {
      gain = div_s(cmax, i);    /* gain(Q15) = cor_max/(cor_max+ener)  */
      g0 = sub(32767, gain);    /* g0(Q15) = 1 - gain */
    }
    else
    {
      g0 =  32767;
      gain = 0;
    }
  }


  for (i = 0; i < L_subfr; i++)
  {
    /* signal_pst[i] = g0*signal[i] + gain*signal[i-t0]; */

    signal_pst[i] = add(mult(g0, signal[i]), mult(gain, signal[i-t0]));

  }

  return;
}
Exemple #29
0
Word16 Pitch_ol(       /* output: open loop pitch lag                        */
   Word16 signal[],    /* input : signal used to compute the open loop pitch */
                       /*     signal[-pit_max] to signal[-1] should be known */
   Word16   pit_min,   /* input : minimum pitch lag                          */
   Word16   pit_max,   /* input : maximum pitch lag                          */
   Word16   L_frame    /* input : length of frame to compute pitch           */
)
{
  Word16  i, j;
  Word16  max1, max2, max3;
  Word16  p_max1, p_max2, p_max3;
  Word32  t0, L_temp;

  /* Scaled signal */

  Word16 scaled_signal[L_FRAME+PIT_MAX];
  Word16 *scal_sig;

  scal_sig = &scaled_signal[pit_max];

  /*--------------------------------------------------------*
   *  Verification for risk of overflow.                    *
   *--------------------------------------------------------*/

   Overflow = 0;
   t0 = 0;

   for(i= -pit_max; i< L_frame; i++)
     t0 = L_mac(t0, signal[i], signal[i]);

  /*--------------------------------------------------------*
   * Scaling of input signal.                               *
   *                                                        *
   *   if Overflow        -> scal_sig[i] = signal[i]>>3     *
   *   else if t0 < 1^20  -> scal_sig[i] = signal[i]<<3     *
   *   else               -> scal_sig[i] = signal[i]        *
   *--------------------------------------------------------*/

   if(Overflow == 1)
   {
     for(i=-pit_max; i<L_frame; i++)
       scal_sig[i] = shr(signal[i], 3);
   }
   else {
     L_temp = L_sub(t0, (Word32)1048576L);
     if ( L_temp < (Word32)0 )  /* if (t0 < 2^20) */
     {
        for(i=-pit_max; i<L_frame; i++)
          scal_sig[i] = shl(signal[i], 3);
     }
     else
     {
       for(i=-pit_max; i<L_frame; i++)
         scal_sig[i] = signal[i];
     }
   }
  /*--------------------------------------------------------------------*
   *  The pitch lag search is divided in three sections.                *
   *  Each section cannot have a pitch multiple.                        *
   *  We find a maximum for each section.                               *
   *  We compare the maximum of each section by favoring small lag.     *
   *                                                                    *
   *  First section:  lag delay = pit_max     downto 4*pit_min          *
   *  Second section: lag delay = 4*pit_min-1 downto 2*pit_min          *
   *  Third section:  lag delay = 2*pit_min-1 downto pit_min            *
   *--------------------------------------------------------------------*/


   j = shl(pit_min, 2);
   p_max1 = Lag_max(scal_sig, L_frame, pit_max, j, &max1);

   i = sub(j, 1); j = shl(pit_min, 1);
   p_max2 = Lag_max(scal_sig, L_frame, i, j, &max2);

   i = sub(j, 1);
   p_max3 = Lag_max(scal_sig, L_frame, i, pit_min , &max3);

  /*--------------------------------------------------------------------*
   * Compare the 3 sections maximum, and favor small lag.               *
   *--------------------------------------------------------------------*/

  if( sub(mult(max1, THRESHPIT), max2)  < 0)
  {
    max1 = max2;
    p_max1 = p_max2;
  }

  if( sub(mult(max1, THRESHPIT), max3)  < 0)
  {
    p_max1 = p_max3;
  }


  return (p_max1);
}
Exemple #30
0
static void Norm_Corr(Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
               Word16 t_min, Word16 t_max, Word16 corr_norm[])
{
  Word16 i,j,k;
  Word16 corr_h, corr_l, norm_h, norm_l;
  Word32 s, L_temp;

  Word16 excf[L_SUBFR];
  Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];


  k =  negate(t_min);

  /* compute the filtered excitation for the first delay t_min */

  Convolve(&exc[k], h, excf, L_subfr);

  /* scaled "excf[]" to avoid overflow */

  for(j=0; j<L_subfr; j++)
    scaled_excf[j] = shr(excf[j], 2);

  /* Compute energy of excf[] for danger of overflow */

  s = 0;
  for (j = 0; j < L_subfr; j++)
    s = L_mac(s, excf[j], excf[j]);

  L_temp = L_sub(s, 67108864L);
  if (L_temp <= 0L)      /* if (s <= 2^26) */
  {
    s_excf = excf;
    h_fac = 15-12;               /* h in Q12 */
    scaling = 0;
  }
  else {
    s_excf = scaled_excf;        /* "excf[]" is divide by 2 */
    h_fac = 15-12-2;             /* h in Q12, divide by 2 */
    scaling = 2;
  }

  /* loop for every possible period */

  for (i = t_min; i <= t_max; i++)
  {
    /* Compute 1/sqrt(energy of excf[]) */

    s = 0;
    for (j = 0; j < L_subfr; j++)
      s = L_mac(s, s_excf[j], s_excf[j]);

    s = Inv_sqrt(s);                     /* Result in Q30 */
    L_Extract(s, &norm_h, &norm_l);

    /* Compute correlation between xn[] and excf[] */

    s = 0;
    for (j = 0; j < L_subfr; j++)
      s = L_mac(s, xn[j], s_excf[j]);

    L_Extract(s, &corr_h, &corr_l);

    /* Normalize correlation = correlation * (1/sqrt(energy)) */

    s = Mpy_32(corr_h, corr_l, norm_h, norm_l);

    corr_norm[i] = extract_h(L_shl(s, 16));   /* Result is on 16 bits */

    /* modify the filtered excitation excf[] for the next iteration */

    if( sub(i, t_max) != 0)
    {
      k=sub(k,1);
      for (j = L_subfr-(Word16)1; j > 0; j--)
      {
        s = L_mult(exc[k], h[j]);
        s = L_shl(s, h_fac);             /* h is in Q(12-scaling) */
        s_excf[j] = add(extract_h(s), s_excf[j-1]);
      }
      s_excf[0] = shr(exc[k], scaling);
    }
  }
  return;
}