Example #1
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;
  
}
Example #2
0
void a2lsp(
           Word16 pc[],       /* (i) Q12: predictor coefficients */
           Word16 lsp[],      /* (o) Q15: line spectral pairs    */
           Word16 old_lsp[])  /* (i) Q15: old lsp                */
{
   Word16 i, j, exp;
   Word16 fa_man[NAB], fa_exp[NAB], fb_man[NAB], fb_exp[NAB];
   Word16 ta_man[NAB], ta_exp[NAB], tb_man[NAB], tb_exp[NAB];
   Word16 *t_man, *t_exp;
   Word32 a0;
   Word16 nd2, nf, ngrd;
   Word16 xroot, xlow, ylow, ind, xhigh, yhigh, xmid, ymid, dx, dy, dxdy, x, sign;


   /* Find normalization for fa and fb */
   /*   fb[0] = fa[0] = 1.0;                             */
   /*   for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) { */
   /*      fa[i] = pc[i] + pc[j] - fa[i-1];              */
   /*      fb[i] = pc[i] - pc[j] + fb[i-1];              */
   /*   }                                                */
   fa_man[0] = 16384; 
   fa_exp[0] = 6;       // fa_man[0] in high 16-bits >> fa_exp[0] = 1.0 in Q24 
   fb_man[0] = 16384;
   fb_exp[0] = 6;       // fb_man[0] in high 16-bits >> fb_exp[0] = 1.0 in Q24
   for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) {
      a0 = L_mult0(pc[i], 4096);     // Q24
      a0 = L_mac0(a0, pc[j], 4096);  // Q24
      a0 = L_sub(a0, L_shr(L_deposit_h(fa_man[i-1]),fa_exp[i-1]));  // Q24
      fa_exp[i] = norm_l(a0);
      fa_man[i] = intround(L_shl(a0, fa_exp[i]));  // Q(8+fb_exp[i])

      a0 = L_mult0(pc[i], 4096);     // Q24
      a0 = L_msu0(a0, pc[j], 4096);  // Q24
      a0 = L_add(a0, L_shr(L_deposit_h(fb_man[i-1]),fb_exp[i-1]));  // Q24
      fb_exp[i] = norm_l(a0);
      fb_man[i] = intround(L_shl(a0, fb_exp[i]));  // Q(8+fb_exp[i])
   }

   nd2 = (LPCO)/2;

   /* ta[] and tb[] in Q(7+exp)               */
   /* ta[0] = fa[nab-1]; ta[i] = 2.0 * fa[j]; */
   /* tb[0] = fb[nab-1]; tb[i] = 2.0 * fb[j]; */
   ta_man[0] = fa_man[NAB-1];
   ta_exp[0] = add(fa_exp[NAB-1], 1);
   tb_man[0] = fb_man[NAB-1];
   tb_exp[0] = add(fb_exp[NAB-1], 1);
   for (i = 1, j = NAB - 2; i < NAB; ++i, --j) {
      ta_man[i] = fa_man[j];
      ta_exp[i] = fa_exp[j];
      tb_man[i] = fb_man[j];
      tb_exp[i] = fb_exp[j];
   }

   nf = 0;
   t_man = ta_man;
   t_exp = ta_exp;
   xroot = 0x7fff;
   ngrd  = 0;
   xlow  = grid[0];  // Q15
   ylow = FNevChebP(xlow, t_man, t_exp, nd2);
   ind = 0;

   /* Root search loop */
   while (ngrd<(Ngrd-1) && nf < LPCO) {
      
      ngrd++;
      xhigh = xlow;
      yhigh = ylow;
      xlow  = grid[ngrd];
      ylow = FNevChebP(xlow, t_man, t_exp, nd2);
      
      if ( L_mult(ylow ,yhigh) <= 0) {
         
         /* Bisections of the interval containing a sign change */
         
         dx = xhigh - xlow;
         for (i = 1; i <= NBIS; ++i) {
            dx = shr(dx, 1);
            xmid = add(xlow, dx);
            ymid = FNevChebP(xmid, t_man, t_exp, nd2);
            if (L_mult(ylow,ymid) <= 0) {
               yhigh = ymid;
               xhigh = xmid;
            } else {
               ylow = ymid;
               xlow = xmid;
            }
         }
         
         /*
         * Linear interpolation in the subinterval with a sign change
         * (take care if yhigh=ylow=0)
         */
         
         dx = sub(xhigh, xlow);
         dy  = sub(ylow, yhigh);
         if (dy != 0) {
            sign = dy;
            dy = abs_s(dy);
            exp = norm_s(dy);
            dy = shl(dy, exp);
            /* The maximum grid distance is 1629 =>                                  */
            /* Maximum dx=1629/2^4=101.8125, i.e. 16384/101.8125=160.92~128 (7 bits) */
            /* However, due to the starting point for the search of a new root,      */
            /* xlow = xroot, 1 more bit of headroom for the division is required.    */
            dxdy = div_s(shl(dx,6), dy); 
            a0 = L_mult(dxdy, ylow);
            a0 = L_shr(a0, sub(6, exp));
            x  = intround(a0);
            if(sign < 0) x = negate(x);
            xmid = add(xlow, x);
         } 
         else {
            xmid = add(xlow, shr(dx,1));
         }
         
         /* acos mapping for New lsp component */
         while (( costable[ind] >= xmid ) && (ind < 63)) ind++;
         ind--;
         a0 = L_mult( sub(xmid, costable[ind]) , acosslope[ind] );
         x  = intround(L_shl(a0, 4));
         lsp[nf] = add(x, shl(ind, 9));
         ++nf;
         
         /* Start the search for the roots of next polynomial at the estimated
         * location of the root just found.  We have to catch the case that the
         * two polynomials have roots at the same place to avoid getting stuck at
         * that root.
         */
         
         if (xmid >= xroot) xmid = xlow - dx;
         xroot = xmid;
         if (t_man == ta_man){
            t_man = tb_man;
            t_exp = tb_exp;
         }
         else{
            t_man = ta_man;
            t_exp = ta_exp;
         }
         xlow = xmid;
         ylow = FNevChebP(xlow, t_man, t_exp, nd2);
         
      }
   }
   
   /* Check if all LSPs are found */
   if( sub(nf, LPCO) < 0)
   {
      W16copy(lsp, old_lsp, LPCO);
   }

   return;
}
Example #3
0
/* Standard Long-Term Postfilter */
void postfilter(
                Word16 *s,   /* input : quantized speech signal         */
                Word16 pp,   /* input : pitch period                    */
                Word16 *ma_a,
                Word16 *b_prv,
                Word16 *pp_prv,
                Word16 *e)   /* output: enhanced speech signal          */
{
   int n;
   Word16 len, t0, t1, t2, t3, shift, aa, R0norm, R0_exp;
   Word32 a0, a1, R0, R1, R01, R01max, Rx;
   Word16 *fp1;
   Word16 ppt, pptmin, pptmax, ppnew;
   Word16 bb[2];
   Word16 R1max_exp, R1max, R01Sqmax_exp, R01Sqmax, R01Sq_exp, R01Sq, R1_exp, R1n;
   Word16 gainn, Rx_exp;
   Word16 buf[MAXPP+FRSZ];
   Word16 *ps, ww1, ww2;
   Word32 step, delta;
   Word16 bi0, bi1c, bi1p;
   
   ps = s+XQOFF;
   
   /********************************************************************/
   /*                 pitch search around decoded pitch                */
   /********************************************************************/
   pptmin = sub(pp, DPPQNS);
   pptmax = add(pp, DPPQNS);
   if (pptmin<MINPP)
   {
      pptmin = MINPP;
      pptmax = add(pptmin, 2*DPPQNS);
   }
   else if (pptmax>MAXPP)
   {
      pptmax = MAXPP;
      pptmin = sub(pptmax, 2*DPPQNS);
   }
   
   fp1 = &s[XQOFF-pptmax];
   len = add(FRSZ, pptmax);
   a0 = 0;
   for (n=0;n<len;n++) 
   {
      t1 = shr(*fp1++, 3);
      a0 = L_mac0(a0,t1,t1);
   }
   shift = norm_l(a0);
   if (a0==0) shift=31;
   shift = sub(6, shift);
   if (shift > 0)
   {
      ps = buf+pptmax;
      fp1 = &s[XQOFF-pptmax];
      shift = shr(add(shift, 1), 1);
      for (n=0;n<len;n++)
      {
         buf[n] = shr(fp1[n], shift);
      }
   }
   else shift=0;
   
   R0  = 0;
   R1  = 0;
   R01 = 0;
   for(n=0; n<FRSZ; n++)
   {
      R0  = L_mac0(R0, ps[n], ps[n]);
      R1  = L_mac0(R1, ps[n-pptmin], ps[n-pptmin]);
      R01 = L_mac0(R01,ps[n], ps[n-pptmin]);
   }
   R0_exp = norm_l(R0);
   R0norm = extract_h(L_shl(R0, R0_exp));
   R0_exp = R0_exp-16;
   
   ppnew        = pptmin;
   R1max_exp    = norm_l(R1);
   R1max        = extract_h(L_shl(R1, R1max_exp));
   R01Sqmax_exp = norm_l(R01);
   t1           = extract_h(L_shl(R01, R01Sqmax_exp));
   R01Sqmax_exp = shl(R01Sqmax_exp, 1);
   R01Sqmax     = extract_h(L_mult(t1, t1));
   R01max       = R01;
   for(ppt=pptmin+1; ppt<=pptmax; ppt++)
   {
      R1 = L_msu0(R1,ps[FRSZ-ppt], ps[FRSZ-ppt]);
      R1 = L_mac0(R1,ps[-ppt], ps[-ppt]);      
      R01= 0;
      for(n=0; n<FRSZ; n++)
      {
         R01 = L_mac0(R01, ps[n], ps[n-ppt]);
      }
      R01Sq_exp = norm_l(R01);
      t1 = extract_h(L_shl(R01, R01Sq_exp));
      R01Sq_exp = shl(R01Sq_exp, 1);
      R01Sq = extract_h(L_mult(t1, t1));
      R1_exp = norm_l(R1);
      R1n = extract_h(L_shl(R1, R1_exp));
      
      a0 = L_mult(R01Sq, R1max);
      a1 = L_mult(R01Sqmax, R1n);
      t1 = add(R01Sq_exp, R1max_exp);
      t2 = add(R01Sqmax_exp, R1_exp);
      
      t2 = sub(t1, t2);
      if (t2>=0) a0 = L_shr(a0, t2);
      if (t2<0)  a1 = L_shl(a1, t2); 
      
      if (L_sub(a0, a1)>0) 
      {
         R01Sqmax = R01Sq; 
         R01Sqmax_exp = R01Sq_exp;
         R1max = R1n; R1max_exp = R1_exp;
         ppnew = ppt;
         R01max = R01;
      }
   }
   
   /******************************************************************/
   /*               calculate all-zero pitch postfilter              */
   /******************************************************************/
   if (R1max==0 || R0==0 || R01max <= 0)
   {
      aa = 0;
   }
   else
   {
      a0 = R1max_exp-16;
      t1 = mult(R1max, R0norm);
      a0 = a0+R0_exp-15;
      sqrt_i(t1, (Word16)a0, &t1, &t2);
      t0 = norm_l(R01max);
      t3 = extract_h(L_shl(R01max, t0));
      t0 = t0-16;
      aa = mult(t3, t1);
      t0 = t0+t2-15;
      t0 = t0-15;
      if (t0<0) aa = shl(aa, sub(0,t0));
      else aa = shr(aa, t0);
   }
   a0 = L_mult(8192, aa);
   a0 = L_mac(a0, 24576, *ma_a);
   *ma_a = intround(a0);
   if((*ma_a < ATHLD1) && (aa < (ATHLD2)))
      aa = 0;
   bb[1] = mult(ScLTPF, aa);
   
   /******************************************************************/
   /*             calculate normalization energies                   */
   /******************************************************************/
   Rx = 0;
   R0 = 0;
   for(n=0; n<FRSZ; n++)
   {
      a0   = L_shl(s[XQOFF+n], 15);
      a0   = L_add(a0, L_mult0(bb[1], s[XQOFF+n-ppnew]));
      e[n] = intround(a0);
      t1   = shr(e[n], shift);
      t2   = shr(s[XQOFF+n], shift);
      Rx   = L_mac0(Rx, t1, t1);
      R0   = L_mac0(R0, t2, t2);
   }
   R0 = L_shr(R0, 2);
   if(R0 == 0 || Rx == 0)
      gainn = 32767;
   else
   {
      Rx_exp = norm_l(Rx);
      t1 = extract_h(L_shl(Rx, Rx_exp));
      t2 = extract_h(L_shl(R0, Rx_exp));
      if (t2>= t1)
         gainn = 32767;
      else
      {
         t1 = div_s(t2, t1);
         gainn = sqrts(t1);
      }
   }
   
   /******************************************************************/
   /*    interpolate from the previous postfilter to the current     */
   /******************************************************************/
   bb[0] = gainn;
   bb[1] = mult(gainn, bb[1]);
   step  = (Word32)((1.0/(NINT+1))*(2147483648.0));
   delta = 0;
   for(n=0; n<NINT; n++)
   {
      delta = L_add(delta, step);
      ww1   = intround(delta);
      ww2   = add(sub(32767, ww1), 1);
      /* interpolate between two filters */
      bi0 = intround(L_mac(L_mult(ww1, bb[0]), ww2, b_prv[0]));
      bi1c= mult(ww1, bb[1]);
      bi1p= mult(ww2, b_prv[1]);
      e[n] = intround(L_mac(L_mac(L_mult(bi1c, s[XQOFF+n-ppnew]), bi1p, s[XQOFF+n-(*pp_prv)]), bi0, s[XQOFF+n]));
   }
   for(n=NINT; n<FRSZ; n++)
   {
      e[n] = intround(L_shl(L_mult(gainn, e[n]),1));
   }
   
   /******************************************************************/
   /*                       save state memory                        */
   /******************************************************************/
   *pp_prv = ppnew;
   b_prv[0] = bb[0];
   b_prv[1] = bb[1];
   
   return;
}
Example #4
0
Word16  refinepitch(
                    Word16 *x,      /* (i) Q1 */
                    Word16    cpp,
                    Word16 *ppt) /* (o) Q9 */
{
   Word32   a0, a1;
   Word32   cor, energy, cormax, enermax32;        /* Q3 */
   Word16   energymax, energymax_exp, ener, ener_exp;
   Word16   cor2, cor2_exp, cor2max, cor2max_exp;
   Word16   *sp0, *sp1, *sp2, *sp3;
   Word16   *xt;
   Word16   s, t;
   Word16   lb, ub;
   int      pp, i, j;
   
   if (cpp >= MAXPP) cpp = MAXPP-1;
   if (cpp < MINPP)  cpp = MINPP;
   lb = sub((Word16)cpp,DEV); 
   if (lb < MINPP) lb = MINPP; /* lower bound of pitch period search range */
   ub = add((Word16)cpp,DEV);
   /* to avoid selecting HMAXPP as the refined pitch period */
   if (ub >= MAXPP) ub = MAXPP-1;/* lower bound of pitch period search range */
   
   i   = lb;            /* start the search from lower bound       */
   xt  = x+XOFF;   
   sp0 = xt;
   sp1 = xt-i;
   cor = energy = 0;
   for (j=0;j<FRSZ; j++) 
   {
      s = *sp1++;
      t = *sp0++;
      energy = L_mac0(energy, s, s);
      cor    = L_mac0(cor, s, t);
   }
   
   pp             = i;
   cormax         = cor;
   enermax32      = energy;
   energymax_exp  = norm_l(enermax32);
   energymax      = extract_h(L_shl(enermax32, energymax_exp));
   a0             = cor;
   cor2max_exp    = norm_l(a0);
   s              = extract_h(L_shl(a0, cor2max_exp));
   cor2max_exp    = shl(cor2max_exp, 1);
   cor2max        = extract_h(L_mult0(s, s));
   sp0            = xt+FRSZ-lb-1;
   sp1            = xt-lb-1;
   for (i=lb+1;i<=ub;i++) 
   {
      sp2 = xt;
      sp3 = xt-i;
      cor = 0;
      for (j=0;j<FRSZ;j++) 
         cor = L_mac0(cor, *sp2++, *sp3++);
      
      a0       = cor;
      cor2_exp = norm_l(a0);
      s        = extract_h(L_shl(a0, cor2_exp));
      cor2_exp = shl(cor2_exp, 1);
      cor2     = extract_h(L_mult0(s, s));
      
      s        = *sp0--;
      t        = *sp1--;
      energy   = L_msu0(energy, s, s);
      energy   = L_mac0(energy, t, t);
      a0       = energy;
      ener_exp = norm_l(a0);
      ener     = extract_h(L_shl(a0, ener_exp));
      
      if (ener>0) 
      {   
         a0 = L_mult0(cor2, energymax);
         a1 = L_mult0(cor2max, ener);
         s  = add(cor2_exp, energymax_exp);
         t  = add(cor2max_exp, ener_exp);
         if (s>=t) a0 = L_shr(a0, sub(s,t));
         else      a1 = L_shr(a1, sub(t,s));
         if (a0 > a1) 
         {
            pp             = i;
            cormax         = cor;
            enermax32      = energy;   
            cor2max        = cor2; 
            cor2max_exp    = cor2_exp;
            energymax      = ener; 
            energymax_exp  = ener_exp;
         }
      }
   }
   
   if ((enermax32 == 0) || (cormax<=0)) 
      *ppt = 0;
   else 
   {
      ub    = sub(norm_l(cormax),1);
      lb    = norm_l(enermax32);
      s     = extract_h(L_shl(cormax,ub));
      t     = extract_h(L_shl(enermax32,lb));
      s     = div_s(s, t);
      lb    = sub(sub(lb,ub),6);
      *ppt  = shl(s, lb);
   }
   return pp;
}