pit_lib.c

MELPe 1200 bps, fixed point

/* Q values                                                                   */
/*     ipitch - Q0, fpitch - Q7, sig_in - Q0, *pcorr - Q14                    */
/*                                                                            */
/* WARNING: this function assumes the input buffer has been normalized by     */
/* f_pitch_scale().                                                           */

Shortword frac_pch(Shortword sig_in[], Shortword *pcorr, Shortword fpitch,
				   Shortword range, Shortword pmin, Shortword pmax,
				   Shortword pmin_q7, Shortword pmax_q7, Shortword lmin)
{
	Shortword	length, cbegin, lower, upper, ipitch;
	Shortword	c0_0, c0_T, c0_T1, cT_T, cT_T1, cT1_T1, c0_Tm1;
	Shortword	shift1a, shift1b, shift2, shift;
	Shortword	frac, frac1, corr;
	Shortword	temp;
	Longword	denom, denom1, denom2, denom3, numer;
	Longword	mag_sq;
	Longword	L_temp1;


	/* Perform local integer pitch search for better fpitch estimate */
	if (range > 0){
		ipitch = shift_r(fpitch, -7);
		lower = sub(ipitch, range);
		upper = add(ipitch, range);
		if (upper > pmax){
			upper = pmax;
		}
		if (lower < pmin){
			lower = pmin;
		}
		if (lower < add(shr(ipitch, 1), shr(ipitch, 2))){
			lower = add(shr(ipitch, 1), shr(ipitch, 2));
		}
		length = ipitch;
		if (length < lmin){
			length = lmin;
		}
		fpitch = shl(find_pitch(sig_in, &corr, lower, upper, length), 7);
	}


	/* Estimate needed crosscorrelations */
	ipitch = shift_r(fpitch, -7);
	if (ipitch >= pmax){
		ipitch = sub(pmax, 1);
	}
	length = ipitch;
	if (length < lmin){
		length = lmin;
	}
	cbegin = negate(shr(add(length, ipitch), 1));

	/* 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));

	if (c0_Tm1 > c0_T1){
		/* fractional component should be less than 1, so decrement pitch */
		c0_T1 = c0_T;
		c0_T = c0_Tm1;
		ipitch = sub(ipitch, 1);
	}
	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))));
	numer = L_sub(L_shr(L_mult(c0_T1, cT_T), 1),
				  L_shr(L_mult(c0_T, cT_T1), 1));

	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 = MINFRAC;
			else
				frac = MAXFRAC;
		} else
			frac = L_divider2(numer, denom, 2, 0);
	} else {
		frac = X05_Q13;
	}
	if (frac > MAXFRAC){
		frac = MAXFRAC;
	}
	if (frac < MINFRAC){
		frac = MINFRAC;
	}

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

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

	/* Calculate denominator */
	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) */

	/* Calculate numerator */
	L_temp1 = L_mult(c0_T, frac1);                                 /* Q(X+14) */
	L_temp1 = L_mac(L_temp1, c0_T1, frac);
	if (L_temp1 <= 0){
		corr = 0;
	} 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   */
	if (corr < temp){
		*pcorr = shr(divide_s(corr, temp), 1);
	} else if (temp <= 0){
		*pcorr = 0;
	} else {
		*pcorr = ONE_Q14;
	}

	/* Return fractional pitch value */
	return(fpitch);
}


/* 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.        */

Shortword p_avg_update(Shortword pitch, Shortword pcorr, Shortword pthresh)
{
	register Shortword	i;
	static Shortword	good_pitch[NF];
	static BOOLEAN	firstTime = TRUE;
	Shortword	pitch_avg, temp;


	if (firstTime){
		fill(good_pitch, DEFAULT_PITCH_Q7, NF);
		firstTime = FALSE;
	}

	/* Strong correlation: update good pitch array */
	if (pcorr > pthresh){
		/* We used to insert pitch to good_pitch[0] and shift everything up.  */
		/* Now we insert at good_pitch[NF - 1] and shift everything down.     */

		v_equ(good_pitch, &(good_pitch[1]), NF - 1);
		good_pitch[NF - 1] = pitch;
	} else {             /* Otherwise decay good pitch array to default value */
		for (i = 0; i < NF; 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 = median3(good_pitch);

	return(pitch_avg);
}


/* 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:                                                                  */
/*    mat.h                                                                   */
/* Organization:                                                              */
/*    Speech Research, Corporate R&D                                          */
/*    Texas Instruments                                                       */
/* Author: Alan McCree                                                        */
/*                                                                            */
/* Copyright (c) 1995 by Texas Instruments, Inc.  All rights reserved.        */
/*                                                                            */
/* 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)
{
	register Shortword	i, section;
	static Shortword	lpres_delin[LPF_ORD];
	static Shortword	lpres_delout[LPF_ORD];
	static Shortword	sigbuf[LPF_ORD + PITCH_FR];
	static BOOLEAN	firstTime = TRUE;
	Shortword	pcorr, pitch;
	Shortword	temp, temp2;
	Shortword	temp_delin[LPF_ORD], temp_delout[LPF_ORD];


	if (firstTime){
		v_zap(lpres_delin, LPF_ORD);
		v_zap(lpres_delout, LPF_ORD);
		firstTime = FALSE;
	}

	/* 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 = (Shortword) (section*2); i < (Shortword) (section*2 + 2);
			 i++){
			temp_delin[i] = lpres_delin[i];
			temp_delout[i] = lpres_delout[i];
		}
		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],
				  PITCH_FR - FRAME);
		/* restore delay buffers for the next overlapping frame */
		for (i = (Shortword) (section*2); i < (Shortword) (section*2 + 2);
			 i++){
			lpres_delin[i] = temp_delin[i];
			lpres_delout[i] = temp_delout[i];
		}
	}

	/* 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,
					MINLENGTH);

	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,
						MINLENGTH);

		if (pcorr < UVMAX) /* If correlation still too low, use average pitch */
			pitch = pitch_avg;
		else {
			/* Else check for pitch doubling (speech thresholds) */
			if (temp > LONG_PITCH)                 /* longer pitches are more */
                                                      /* likely to be doubles */
				temp2 = PDOUBLE4;
			else
				temp2 = PDOUBLE3;
			pitch = double_chk(&sigbuf[LPF_ORD + (PITCH_FR/2)], &pcorr,
							   temp, temp2, PITCHMIN, PITCHMAX, PITCHMIN_Q7,
							   PITCHMAX_Q7, MINLENGTH);
		}
	} else {

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

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

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