pstcp.c

语音编码G.729 语音编码G.729

/*
   ITU-T G.729 Annex C+ - Reference C code for floating point
                         implementation of G.729 Annex C+
                         (integration of Annexes B, D and E)
                          Version 2.1 of October 1999
*/

/*
 File : PSTCP.C
*/

/************************************************************************/
/*      Post - filtering : short term + long term                       */
/************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/**** Prototypes                                                        */
#include "typedef.h"
#include "ld8k.h"
#include "ld8cp.h"
#include "tab_ld8k.h"

/* Prototypes for local functions */
/**********************************/

static void pst_ltpe(int t0, FLOAT *ptr_sig_in,
    FLOAT *ptr_sig_pst0, int *vo, FLOAT gamma_harm);
static void search_del(int t0, FLOAT *ptr_sig_in, int *ltpdel, int *phase,
    FLOAT *num_gltp, FLOAT *den_gltp, FLOAT *y_up, int *off_yup);
static void filt_plt( FLOAT *s_in, FLOAT *s_ltp,
    FLOAT *s_out, FLOAT gain_plt);
static void compute_ltp_l(FLOAT *s_in, int ltpdel, int phase,
    FLOAT *y_up, FLOAT *num, FLOAT *den);
static int select_ltp(FLOAT num1, FLOAT den1, FLOAT num2, FLOAT den2);
static void calc_st_filte(struct postfilt_state_t *state, 
	FLOAT *apond2, FLOAT *apond1, FLOAT *parcor0,
    FLOAT *signal_ltp0_ptr, int  long_h_st, int m_pst);
static void filt_mu(FLOAT *sig_in, FLOAT *sig_out, FLOAT parcor0);
static void calc_rc0_he(FLOAT *h, FLOAT *rc0, int  long_h_st);
static void scale_st(FLOAT *sig_in, FLOAT *sig_out, FLOAT *gain_prec);


/************************************************************************/
/****   Short term postfilter :                                     *****/
/*      Hst(z) = Hst0(z) Hst1(z)                                        */
/*      Hst0(z) = 1/g0 A(gamma2)(z) / A(gamma1)(z)                      */
/*      if {hi} = i.r. filter A(gamma2)/A(gamma1) (truncated)           */
/*      g0 = SUM(|hi|) if > 1                                           */
/*      g0 = 1. else                                                    */
/*      Hst1(z) = 1/(1 - |mu|) (1 + mu z-1)                             */
/*      with mu = k1 * gamma3                                           */
/*      k1 = 1st parcor calculated on {hi}                              */
/*      gamma3 = gamma3_minus if k1<0, gamma3_plus if k1>0              */
/****   Long term postfilter :                                      *****/
/*      harmonic postfilter :   H0(z) = gl * (1 + b * z-p)              */
/*      b = gamma_g * gain_ltp                                          */
/*      gl = 1 / 1 + b                                                  */
/*      computation of delay p on A(gamma2)(z) s(z)                     */
/*      sub optimal search                                              */
/*      1. search around 1st subframe delay (3 integer values)          */
/*      2. search around best integer with fract. delays (1/8)          */
/************************************************************************/
/*----------------------------------------------------------------------------
 * Init_Post_Filter -  Initialize postfilter functions
 *----------------------------------------------------------------------------
 */
void init_post_filter(struct postfilt_state_t *state)
{
    /* Initialize arrays and pointers */

   /* A(gamma2) residual */
	set_zero(state->res2, MEM_RES2);
    state->res2_ptr = state->res2 + MEM_RES2;

    /* 1/A(gamma1) memory */
	set_zero(state->mem_stp, M);
    state->ptr_mem_stp = state->mem_stp + M - 1;

    /* fill apond2[M+1->LONG_H_ST-1] with zeroes */
	set_zero(state->apond2 + MP1, LONG_H_ST - MP1);

    /* null memory to compute i.r. of A(gamma2)/A(gamma1) */
	set_zero(state->mem_zero, M);

    /* for gain adjustment */
    state->gain_prec = (F)1.;
}

/*----------------------------------------------------------------------------
 * Post - adaptive postfilter main function
 *----------------------------------------------------------------------------
 */
void poste(struct postfilt_state_t *state,
    int   t0,             /* input : pitch delay given by coder */
    FLOAT *signal_ptr,    /* input : input signal (pointer to current subframe */
    FLOAT *coeff,         /* input : LPC coefficients for current subframe */
    FLOAT *sig_out,       /* output: postfiltered output */
    int   *vo,            /* output: voicing decision 0 = uv,  > 0 delay */
    FLOAT gamma1,         /* input: short term postfilt. den. weighting factor*/
    FLOAT gamma2,         /* input: short term postfilt. num. weighting factor*/
    FLOAT gamma_harm,     /* input: long term postfilter weighting factor*/
    int   long_h_st,      /* input: impulse response length*/
    int   m_pst,          /* input:  LPC order */
    int Vad               /* input : VAD information (frame type)  */
)
{
    /* Local variables and arrays */
    FLOAT apond1[M_BWDP1];             /* s.t. denominator coeff.      */
    FLOAT sig_ltp[L_SUBFRP1];      /* H0 output signal             */
    FLOAT *sig_ltp_ptr;
    FLOAT parcor0;

    /* Compute weighted LPC coefficients */
    weight_az(coeff, gamma1, m_pst, apond1);
    weight_az(coeff, gamma2, m_pst, state->apond2);
    set_zero(&state->apond2[m_pst+1], (M_BWD-m_pst));

    /* Compute A(gamma2) residual */
    residue(m_pst, state->apond2, signal_ptr, state->res2_ptr, L_SUBFR);

    /* Harmonic filtering */
    sig_ltp_ptr = sig_ltp + 1;

    if (Vad > 1)
      pst_ltpe(t0, state->res2_ptr, sig_ltp_ptr, vo, gamma_harm);
    else {
      *vo = 0;
      copy(state->res2_ptr, sig_ltp_ptr, L_SUBFR);
    }

    /* Save last output of 1/A(gamma1)  */
    /* (from preceding subframe)        */
    sig_ltp[0] = *state->ptr_mem_stp;

    /* Controls short term pst filter gain and compute parcor0   */
    calc_st_filte(state, state->apond2, apond1, &parcor0, sig_ltp_ptr, long_h_st, m_pst);

    /* 1/A(gamma1) filtering, mem_stp is updated */
    syn_filte(m_pst, apond1, sig_ltp_ptr, sig_ltp_ptr, L_SUBFR,
            &state->mem_stp[M_BWD-m_pst], 0);
    copy(&sig_ltp_ptr[L_SUBFR-M_BWD], state->mem_stp, M_BWD);

    /* Tilt filtering */
    filt_mu(sig_ltp, sig_out, parcor0);

    /* Gain control */
    scale_st(signal_ptr, sig_out, &state->gain_prec);

    /**** Update for next subframe */
    copy(&state->res2[L_SUBFR], &state->res2[0], MEM_RES2);
}

/*----------------------------------------------------------------------------
 * post - adaptive postfilter main function
 *----------------------------------------------------------------------------
 */
void post(struct postfilt_state_t *state,
	int t0,                /* input : pitch delay given by coder */
	FLOAT *signal_ptr,     /* input : input signal (pointer to current subframe */
	FLOAT *coeff,          /* input : LPC coefficients for current subframe */
	FLOAT *sig_out,        /* output: postfiltered output */
	int *vo,               /* output: voicing decision 0 = uv,  > 0 delay */
	int Vad                /* input : frame type */
)
{
    FLOAT apond1[MP1];           /* s.t. denominator coeff.      */
    FLOAT sig_ltp[L_SUBFRP1];   /* H0 output signal             */
    FLOAT *sig_ltp_ptr;
    FLOAT parcor0;

    /* Compute weighted LPC coefficients */
    weight_az(coeff, GAMMA1_PST, M, apond1);
    weight_az(coeff, GAMMA2_PST, M, state->apond2);

    /* Compute A(gamma2) residual */
    residue(M, state->apond2, signal_ptr, state->res2_ptr, L_SUBFR);

    /* Harmonic filtering */
    sig_ltp_ptr = sig_ltp + 1;
    if (Vad == 1)
      pst_ltpe(t0, state->res2_ptr, sig_ltp_ptr, vo, GAMMA_HARM);
    else {
      *vo = 0;
      copy(state->res2_ptr, sig_ltp_ptr, L_SUBFR);
    }

    /* Save last output of 1/A(gamma1)  */
    /* (from preceding subframe)        */
    sig_ltp[0] = *state->ptr_mem_stp;

    /* Control short term pst filter gain and compute parcor0   */
    calc_st_filte(state, state->apond2, apond1, &parcor0, sig_ltp_ptr, LONG_H_ST, M);

    /* 1/A(gamma1) filtering, mem_stp is updated */
    syn_filte(M, apond1, sig_ltp_ptr, sig_ltp_ptr, L_SUBFR, state->mem_stp, 1);

    /* (1 + mu z-1) tilt filtering */
    filt_mu(sig_ltp, sig_out, parcor0);

    /* gain control */
    scale_st(signal_ptr, sig_out, &state->gain_prec);

    /**** Update for next frame */
    copy(&state->res2[L_SUBFR], &state->res2[0], MEM_RES2);
}

/*----------------------------------------------------------------------------
 *  pst_ltp - harmonic postfilter
 *----------------------------------------------------------------------------
 */
static void pst_ltpe(
    int t0,             /* input : pitch delay given by coder */
    FLOAT *ptr_sig_in,    /* input : postfilter input filter (residu2) */
    FLOAT *ptr_sig_pst0,  /* output: harmonic postfilter output */
    int *vo,             /* output: voicing decision 0 = uv,  > 0 delay */
    FLOAT gamma_harm      /* input: harmonic postfilter coefficient */
)
{

/**** Declare variables                                 */
    int ltpdel, phase;
    FLOAT num_gltp, den_gltp;
    FLOAT num2_gltp, den2_gltp;
    FLOAT gain_plt;
    FLOAT y_up[SIZ_Y_UP];
    FLOAT *ptr_y_up;
    int off_yup;

    /* Sub optimal delay search */
    search_del(t0, ptr_sig_in, &ltpdel, &phase, &num_gltp, &den_gltp,
                            y_up, &off_yup);
    *vo = ltpdel;

    if(num_gltp == (F)0.)   {
        copy(ptr_sig_in, ptr_sig_pst0, L_SUBFR);
    }

    else {
        if(phase == 0) {
            ptr_y_up = ptr_sig_in - ltpdel;
        }

        else {
            /* Filtering with long filter */
            compute_ltp_l(ptr_sig_in, ltpdel, phase, ptr_sig_pst0,
                &num2_gltp, &den2_gltp);

            if(select_ltp(num_gltp, den_gltp, num2_gltp, den2_gltp) == 1){

                /* select short filter */
                ptr_y_up = y_up + ((phase-1) * L_SUBFRP1 + off_yup);
            }
            else {
                /* select long filter */
                num_gltp = num2_gltp;
                den_gltp = den2_gltp;
                ptr_y_up = ptr_sig_pst0;
            }

        }

        if(num_gltp >= den_gltp) {
            /* beta bounded to 1 */
            if (gamma_harm == GAMMA_HARM)   gain_plt = MIN_GPLT;
            else gain_plt = (F)1./((F)1.+ gamma_harm);
        }
        else {
            gain_plt = den_gltp / (den_gltp + gamma_harm * num_gltp);
        }
        
        /** filtering by H0(z) = harmonic filter **/
        filt_plt(ptr_sig_in, ptr_y_up, ptr_sig_pst0, gain_plt);
        
    }
}

/*----------------------------------------------------------------------------
 *  search_del: computes best (shortest) integer LTP delay + fine search
 *----------------------------------------------------------------------------
 */
static void search_del(
    int t0,                /* input : pitch delay given by coder */
    FLOAT *ptr_sig_in,     /* input : input signal (with delay line) */
    int *ltpdel,           /* output: delay = *ltpdel - *phase / f_up */
    int *phase,            /* output: phase */
    FLOAT *num_gltp,       /* output: numerator of LTP gain */
    FLOAT *den_gltp,       /* output: denominator of LTP gain */
    FLOAT *y_up,           /*       : */
    int *off_yup           /*       : */
)
{
    /* pointers on tables of constants */
    const FLOAT *ptr_h;

    /* Variables and local arrays */
    FLOAT tab_den0[F_UP_PST-1], tab_den1[F_UP_PST-1];
    FLOAT *ptr_den0, *ptr_den1;
    FLOAT *ptr_sig_past, *ptr_sig_past0;
    FLOAT *ptr1;

    int i, n, ioff, i_max;
    FLOAT ener, num, numsq, den0, den1;
    FLOAT den_int, num_int;
    FLOAT den_max, num_max, numsq_max;
    int phi_max;
    int lambda, phi;
    FLOAT temp0, temp1;
    FLOAT *ptr_y_up;

    /*****************************************/
    /* Compute current signal energy         */
    /*****************************************/

    ener = (F)0.;
    for(i=0; i<L_SUBFR; i++) {
        ener += ptr_sig_in[i] * ptr_sig_in[i];
    }
    if(ener < (F)0.1) {
        *num_gltp = (F)0.;
        *den_gltp = (F)1.;
        *ltpdel = 0;
        *phase = 0;
        return;
    }

    /*************************************/
    /* Selects best of 3 integer delays  */
    /* Maximum of 3 numerators around t0 */
    /* coder LTP delay                   */
    /*************************************/
    lambda = t0 - 1;

    ptr_sig_past = ptr_sig_in - lambda;

    num_int = (F)-1.0e30;

   /* initialization used only to suppress Microsoft Visual C++ warnings */
    i_max = 0;
    for(i=0; i<3; i++) {
        num=(F)0.;
        for(n=0; n<L_SUBFR; n++) {
            num += ptr_sig_in[n]* ptr_sig_past[n];
        }
        if(num > num_int) {
            i_max   = i;
            num_int = num;
        }
        ptr_sig_past--;
    }
    if(num_int <= (F)0.) {
        *num_gltp = (F)0.;
        *den_gltp = (F)1.;
        *ltpdel   = 0;
        *phase    = 0;
        return;
    }

    /* Calculates denominator for lambda_max */
    lambda += i_max;
    ptr_sig_past = ptr_sig_in - lambda;
    den_int=(F)0.;
    for(n=0; n<L_SUBFR; n++) {
        den_int += ptr_sig_past[n]* ptr_sig_past[n];
    }
    if(den_int < (F)0.1) {
        *num_gltp = (F)0.;
        *den_gltp = (F)1.;
        *ltpdel   = 0;
        *phase    = 0;
        return;
    }
    /***********************************/
    /* Select best phase around lambda */
    /***********************************/

    /* Compute y_up & denominators */
    /*******************************/
    ptr_y_up = y_up;
    den_max = den_int;
    ptr_den0 = tab_den0;
    ptr_den1 = tab_den1;
    ptr_h = tab_hup_s;
    ptr_sig_past0 = ptr_sig_in + LH_UP_S - 1 - lambda; /* points on lambda_max+1 */

    /* loop on phase  */
    for(phi=1; phi<F_UP_PST; phi++) {

        /* Computes criterion for (lambda_max+1) - phi/F_UP_PST     */
        /* and lambda_max - phi/F_UP_PST                            */
        ptr_sig_past = ptr_sig_past0;
        /* computes y_up[n] */
        for(n = 0; n<=L_SUBFR; n++) {
            ptr1 = ptr_sig_past++;
            temp0 = (F)0.;
            for(i=0; i<LH2_S; i++) {