pit_lib.c

2400bps MELP语音编解码源程序。

    if (length < lmin) {
      length = lmin;    data_move();
    }
    cbegin = negate(shr(add(length,ipitch),1));    data_move();

    /* Calculate normalization for numerator and denominator */
    mag_sq = L_v_magsq(&sig_in[cbegin],length,0,1);
    shift1a = norm_s(extract_h(mag_sq));
    shift1b = norm_s(extract_h(
	L_v_magsq(&sig_in[cbegin+ipitch-1],(Shortword)(length+2),0,1)));
    shift = add(shift1a,shift1b);
    shift2 = shr(shift,1);  /* shift2 = half of total shift */
    if (shl(shift2,1) != shift)
	shift1a = sub(shift1a,1);

    /* Calculate correlations with appropriate normalization */
    c0_0 = extract_h(L_shl(mag_sq,shift1a));

    c0_T = extract_h(L_shl(L_v_inner(&sig_in[cbegin],
				     &sig_in[cbegin+ipitch],length,
				     0,0,1),shift2));
    c0_T1 = extract_h(L_shl(L_v_inner(&sig_in[cbegin],
				      &sig_in[cbegin+ipitch+1],
				      length,0,0,1),shift2));
    c0_Tm1 = extract_h(L_shl(L_v_inner(&sig_in[cbegin],
				       &sig_in[cbegin+ipitch-1],
				       length,0,0,1),shift2));

    /* increment complexity for if statement */
    compare_nonzero();
    if (c0_Tm1 > c0_T1) {
        /* fractional component should be less than 1, so decrement pitch */
        c0_T1 = c0_T;    data_move();
        c0_T = c0_Tm1;    data_move();
        ipitch = sub(ipitch,1) ;    data_move();
    }
    cT_T1 = extract_h(L_shl(L_v_inner(&sig_in[cbegin+ipitch],
				      &sig_in[cbegin+ipitch+1],length,
				      0,0,1),shift1b));
    cT_T = extract_h(L_shl(L_v_inner(&sig_in[cbegin+ipitch],
				     &sig_in[cbegin+ipitch],length,
				     0,0,1),shift1b));
    cT1_T1 = extract_h(L_shl(L_v_inner(&sig_in[cbegin+ipitch+1],
				       &sig_in[cbegin+ipitch+1],length,
				       0,0,1),shift1b));

    /* Find fractional component of pitch within integer range */
    /* frac = Q13 */
    denom = L_add(L_mult(c0_T1,sub(shr(cT_T,1),shr(cT_T1,1))),
                  L_mult(c0_T,sub(shr(cT1_T1,1),shr(cT_T1,1))));
        L_data_move();

    numer = L_sub(L_shr(L_mult(c0_T1,cT_T),1), L_shr(L_mult(c0_T,cT_T1),1)); 
        L_data_move();
    
    /* increment complexity for if statement */
    compare_zero();
    L_temp1 = L_abs(denom);
    if (L_temp1 > 0) {
      if (L_abs(L_shr(numer,2)) > L_temp1) {
	if (((numer>0)&&(denom<0)) || ((numer<0)&&(denom>0)))
	  frac = (Shortword)MINFRAC;
	else
	  frac = (Shortword)MAXFRAC;
      }
      else
	frac = L_divider2(numer,denom,2,0);    data_move();
    }
    else {
      frac = (Shortword)X05_Q13;    data_move();
    }
    /* increment complexity for if statement */
    compare_nonzero();
    if (frac > MAXFRAC) {
      frac = (Shortword)MAXFRAC;    data_move();
    }
    /* increment complexity for if statement */
    compare_nonzero();
    if (frac < MINFRAC) {
      frac = (Shortword)MINFRAC;    data_move();
    }

    
    /* Make sure pitch is still within range */
    fpitch = add(shl(ipitch,7), shr(frac,6));      data_move();
    /* increment complexity for if statement */
    compare_nonzero();
    if (fpitch > pmax_q7) {
      fpitch = pmax_q7;    data_move();
      frac = shl(sub(fpitch, shl(ipitch,7)),6);     data_move();
    }
    /* increment complexity for if statement */
    compare_nonzero();
    if (fpitch < pmin_q7) {
      fpitch = pmin_q7;    data_move();
      frac = shl(sub(fpitch, shl(ipitch,7)),6);     data_move();
    }
    

    /* Calculate interpolated correlation strength */
    frac1 = sub(ONE_Q13 ,frac);    data_move();

    /* Calculate denominator */
    data_move();   data_move();   data_move();   data_move();
    denom1 = L_shr(L_mpy_ls(L_mult(cT_T,frac1),frac1),1);  /* Q(X+11) */
    denom2 = L_mpy_ls(L_mult(cT_T1,frac1),frac); 
    denom3 = L_shr(L_mpy_ls(L_mult(cT1_T1,frac),frac),1);  /* Q(X+11) */
    denom = L_mpy_ls(L_add(L_add(denom1,denom2),denom3),c0_0); /* Q(2X-4) */ 
    temp = L_sqrt_fxp(denom,0);     /* temp in Q(X-2) */   data_move();

    /* Calculate numerator */
    L_temp1 = L_mult(c0_T,frac1);         /* Q(X+14) */
    L_temp1 = L_mac(L_temp1,c0_T1,frac);
    compare_zero();
    if (L_temp1 <= 0) {
	corr = 0;    data_move();
    }
    else {
        corr = extract_h(L_temp1);
    }

    /* Q value of *pcorr =       Q(L_temp1)                 X+14
                               - extract_h                -   16
			       + scale in divide_s()      +   15
			       - Q(temp)                  -(X-2)
			                                  =   15   */
    /* increment complexity for if statement */
    compare_nonzero();
    if (corr < temp) {
	*pcorr = shr(divide_s(corr,temp),1);    data_move();
    }
    else if (temp <= 0) {
	*pcorr = 0;          data_move();
    }
    else {
	*pcorr = ONE_Q14;    data_move();
    }
    
    /* Return fractional pitch value */
    return(fpitch);

}
#undef MAXFRAC
#undef MINFRAC

/*
    Name: p_avg_update.c
    Description: Update pitch average value.
    Inputs:
      pitch - current pitch value
      pcorr - correlation strength at current pitch value
      pthresh - pitch correlation threshold
    Returns: pitch_avg - updated average pitch value

    Copyright (c) 1995 by Texas Instruments, Inc.  All rights reserved.
*/

/* Static constants */
static Shortword PDECAY_Q15;
static Shortword DEFAULT_PITCH_Q7;
static Shortword PDECAY_PITCH_Q7;
static Shortword NUM_GOOD;

/* Static data */
static Shortword *good_pitch;

Shortword p_avg_update(Shortword pitch, Shortword pcorr, Shortword pthresh)

{
    Shortword i;
    Shortword pitch_avg;
    Shortword temp;

    /* Strong correlation: update good pitch array */
    if (pcorr > pthresh) {
	for (i = NUM_GOOD-1; i >= 1; i--)
	  good_pitch[i] = good_pitch[i-1];
	good_pitch[0] = pitch;
    }
    
    /* Otherwise decay good pitch array to default value */
    else {
      for (i = 0; i < NUM_GOOD; i++) {
	  /* good_pitch[i] = 
	         (PDECAY*good_pitch[i]) +((1.0-PDECAY)*DEFAULT_PITCH); */
	  temp = mult(PDECAY_Q15,good_pitch[i]);
	  good_pitch[i] = add(temp,PDECAY_PITCH_Q7);
      }
    }
    
    /* Pitch_avg = median of pitch values */
    pitch_avg = median(good_pitch,NUM_GOOD);

    return(pitch_avg);
}


void p_avg_init(Shortword pdecay, Shortword default_pitch, 
		Shortword pdecay_pitch, Shortword num_good)

{

    /* Initialize constants */
    PDECAY_Q15 = pdecay;
    DEFAULT_PITCH_Q7 = default_pitch;
    PDECAY_PITCH_Q7 = pdecay_pitch;
    NUM_GOOD = num_good;

    /* Allocate and initialize good pitch array */
    MEM_ALLOC(malloc,good_pitch,NUM_GOOD,Shortword);
    fill(good_pitch,DEFAULT_PITCH_Q7,NUM_GOOD);

}



/*
    Name: pitch_ana.c
    Description: Pitch analysis - outputs candidates
    Inputs:
      resid[] - input residual signal
      pitch_est - initial (floating point) pitch estimate
    Outputs: 
      pitch_cand - pitch candidates for frame
      pcorr - pitch correlation strengths for candidates
    Returns: void
    See_Also:
    Includes:
        spbstd.h
	mat.h
    Organization:
         Speech Research, Corporate R&D
         Texas Instruments
    Author: Alan McCree

    Copyright (c) 1995 by Texas Instruments, Inc.  All rights reserved.
*/

/* Compiler constants */
/* Added 1 to variables which appear in comparison statements to make it
   bit-exact as tested version */
#define UVMAX       (9011+1)  /* (0.55*(1<<14)) */     /* Q14 */
#define PCORR_THR   (9830+1)  /* (0.6*(1<<14)) */      /* Q14 */
#define PDOUBLE1    96        /* (0.75*(1<<7)) */      /* Q7 */
#define PDOUBLE2    64        /* (0.5*(1<<7)) */       /* Q7 */
#define PDOUBLE3    115       /* (0.9*(1<<7)) */       /* Q7 */
#define PDOUBLE4    89        /* (0.7*(1<<7)) */       /* Q7 */
#define LONG_PITCH  12800     /* (100.0*(1<<7)) */     /* Q7 */
#define LPF_ORD_SOS 2

/* Static constants */
static Shortword PITCHMAX;
static Shortword PITCHMIN;
static Shortword PITCHMAX_Q7;
static Shortword PITCHMIN_Q7;
static Shortword FRAME;
static Shortword LPF_ORD;
static Shortword PITCH_FR;
static Shortword LMIN;

/* External variables */
extern Shortword lpf_num[], lpf_den[];

/* Static data */
static Shortword *lpres_delin;
static Shortword *lpres_delout;
static Shortword *sigbuf;

/* Q values 
   --------
   speech - Q0
   resid - Q0
   pitch_est - Q7
   pitch_avg - Q7
   pcorr2 - Q14
*/

Shortword pitch_ana(Shortword speech[],Shortword resid[],
		    Shortword pitch_est,Shortword pitch_avg, 
		    Shortword *pcorr2)
{

    Shortword i,section;
    Shortword temp, temp2, pcorr, pitch;
    Shortword *lpf_res;
    Shortword *temp_delin;
    Shortword *temp_delout;
    

    /* Allocate and initialize delay memory */
    MEM_ALLOC(malloc,temp_delin,LPF_ORD,Shortword);
    MEM_ALLOC(malloc,temp_delout,LPF_ORD,Shortword);

    /* Lowpass filter residual signal */
    v_equ(&sigbuf[LPF_ORD_SOS],&resid[-PITCHMAX],PITCH_FR);

    for (section=0; section<LPF_ORD/2; section++) {
      iir_2nd_s(&sigbuf[LPF_ORD_SOS],&lpf_den[section*3],
		&lpf_num[section*3],&sigbuf[LPF_ORD_SOS],
		&lpres_delin[section*2],&lpres_delout[section*2],FRAME);
      /* save delay buffers for the next overlapping frame */
      for (i = section*2; i < section*2+2; i++) {
        temp_delin[i] = lpres_delin[i];      data_move();
        temp_delout[i] = lpres_delout[i];      data_move();
      }
      iir_2nd_s(&sigbuf[LPF_ORD_SOS+FRAME],&lpf_den[section*3],
                &lpf_num[section*3],&sigbuf[LPF_ORD_SOS+FRAME],
                &lpres_delin[section*2],&lpres_delout[section*2],
		(Shortword)(PITCH_FR-FRAME));
      /* restore delay buffers for the next overlapping frame */
      for (i = section*2; i < section*2+2; i++) {
        lpres_delin[i] = temp_delin[i];      data_move();
        lpres_delout[i] = temp_delout[i];      data_move();
      }
    }

    lpf_res = &sigbuf[LPF_ORD_SOS+(PITCH_FR/2)];  

    /* Scale lowpass residual for pitch correlations */
    f_pitch_scale(&sigbuf[LPF_ORD_SOS],&sigbuf[LPF_ORD_SOS],PITCH_FR);

    /* Perform local search around pitch estimate */
    temp = frac_pch(&sigbuf[LPF_ORD_SOS+(PITCH_FR/2)],&pcorr,
		    pitch_est,5,PITCHMIN,PITCHMAX,PITCHMIN_Q7,
		    PITCHMAX_Q7,LMIN);

    if (pcorr < PCORR_THR) {

      /* If correlation is too low, try speech signal instead */
      v_equ(&sigbuf[LPF_ORD],&speech[-PITCHMAX],PITCH_FR);

      /* Scale speech for pitch correlations */
      f_pitch_scale(&sigbuf[LPF_ORD_SOS],&sigbuf[LPF_ORD_SOS],PITCH_FR);

      temp = frac_pch(&sigbuf[LPF_ORD+(PITCH_FR/2)],&pcorr,
		      pitch_est,0,PITCHMIN,PITCHMAX,PITCHMIN_Q7,
		      PITCHMAX_Q7,LMIN);

      if (pcorr < UVMAX)		
	/* If correlation still too low, use average pitch */
	pitch = pitch_avg;

      else {

	/* Else check for pitch doubling (speech thresholds) */
	temp2 = (Shortword)PDOUBLE3;
	if (temp > LONG_PITCH)
	  /* longer pitches are more likely to be doubles */
	  temp2 = (Shortword)PDOUBLE4;
	pitch = double_chk(&sigbuf[LPF_ORD+(PITCH_FR/2)],&pcorr,
			   temp,temp2,PITCHMIN,PITCHMAX,PITCHMIN_Q7,
			   PITCHMAX_Q7,LMIN);
      }
    }

    else {

      /* Else check for pitch doubling (residual thresholds) */
      temp2 = (Shortword)PDOUBLE1;
      if (temp > LONG_PITCH)

	/* longer pitches are more likely to be doubles */
	temp2 = (Shortword)PDOUBLE2;

      pitch = double_chk(&sigbuf[LPF_ORD+(PITCH_FR/2)],&pcorr,
			 temp,temp2,PITCHMIN,PITCHMAX,PITCHMIN_Q7,
			 PITCHMAX_Q7,LMIN);
    }

    if (pcorr < UVMAX) {

	/* If correlation still too low, use average pitch */
	pitch = pitch_avg;
    }

    MEM_FREE(FREE,temp_delin);
    MEM_FREE(FREE,temp_delout);

    /* Return pitch and set correlation strength */
    *pcorr2 = pcorr;
    return(pitch);

} /* pitch_ana */


void pitch_ana_init(Shortword pmin,Shortword pmax,Shortword fr,
		    Shortword lpf_ord, Shortword lmin)
{

    /* Initialize constants */
    FRAME = fr;
    PITCHMIN = pmin;
    PITCHMAX = pmax;
    PITCHMIN_Q7 = shl(pmin,7);
    PITCHMAX_Q7 = shl(pmax,7);
    LPF_ORD = lpf_ord;
    LMIN = lmin;
    PITCH_FR = ((2*PITCHMAX)+1);


    /* Allocate and initialize delay memory */
    MEM_ALLOC(malloc,lpres_delin,LPF_ORD,Shortword);
    MEM_ALLOC(malloc,lpres_delout,LPF_ORD,Shortword);
    v_zap(lpres_delin,LPF_ORD);
    v_zap(lpres_delout,LPF_ORD);

    /* Allocate scratch buffer */
    MEM_ALLOC(malloc,sigbuf,(LPF_ORD+PITCH_FR),Shortword);
}