pstcp.c

语音压缩编码中的g729p编码程序

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

/* Arrays */
static FLOAT apond2[LONG_H_ST_E];    /* s.t. numerator coeff.        */
static FLOAT mem_stp[M_BWD];           /* s.t. postfilter memory       */
static FLOAT mem_zero[M_BWD];          /* null memory to compute h_st  */
static FLOAT res2[SIZ_RES2];       /* A(gamma2) residual           */

/* pointers */
FLOAT *res2_ptr;
FLOAT *ptr_mem_stp;

/* Variables */
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(void)
{
    int i;

    /* Initialize arrays and pointers */

    /* res2 =  A(gamma2) residual */
    for(i=0; i<MEM_RES2; i++) res2[i] = (F)0.;
    res2_ptr = res2 + MEM_RES2;

    /* 1/A(gamma1) memory */
    for(i=0; i<M_BWD; i++) mem_stp[i] = (F)0.;
    ptr_mem_stp = mem_stp + M_BWD - 1;

    /* fill apond2[M+1->long_h_st-1] with zeroes */
    for(i=M_BWDP1; i<LONG_H_ST_E; i++) apond2[i] = (F)0.;

    /* null memory to compute i.r. of A(gamma2)/A(gamma1) */
    for(i=0; i<M_BWD; i++) mem_zero[i] = (F)0.;

    /* for gain adjustment */
    gain_prec = (F)1.;

    return;
}

/*----------------------------------------------------------------------------
 * Post - adaptive postfilter main function
 *----------------------------------------------------------------------------
 */
void poste(
    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, apond2);
    set_zero(&apond2[m_pst+1], (M_BWD-m_pst));

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

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

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

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

    /* Controls short term pst filter gain and compute parcor0   */
    calc_st_filte(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,
            &mem_stp[M_BWD-m_pst], 0);
    copy(&sig_ltp_ptr[L_SUBFR-M_BWD], mem_stp, M_BWD);

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

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

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

    return;
}

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

    return;
}

/*----------------------------------------------------------------------------
 *  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 */
    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++) {
                temp0 += ptr_h[i] * ptr1[-i];
            }
            ptr_y_up[n] = temp0;
        }

        /* recursive computation of den0 (lambda_max+1) and den1 (lambda_max) */

        /* common part to den0 and den1 */
        temp0 = (F)0.;
        for(n=1; n<L_SUBFR; n++) {
            temp0 += ptr_y_up[n] * ptr_y_up[n];
        }

        /* den0 */
        den0  = temp0 + ptr_y_up[0] * ptr_y_up[0];
        *ptr_den0++ = den0;

        /* den1 */
        den1 = temp0 + ptr_y_up[L_SUBFR] * ptr_y_up[L_SUBFR];
        *ptr_den1++ = den1;

        if(fabs(ptr_y_up[0])>fabs(ptr_y_up[L_SUBFR])) {