Exemple #1
0
static Word16
MR795_gain_code_quant_mod(  /* o  : index of quantization.            */
    Word16 gain_pit,        /* i  : pitch gain,                   Q14 */
    Word16 exp_gcode0,      /* i  : predicted CB gain (exponent), Q0  */
    Word16 gcode0,          /* i  : predicted CB gain (norm.),    Q14 */
    Word16 frac_en[],       /* i  : energy coefficients (4),
                                    fraction part,                Q15 */
    Word16 exp_en[],        /* i  : energy coefficients (4),
                                    eponent part,                 Q0  */
    Word16 alpha,           /* i  : gain adaptor factor (>0),     Q15 */
    Word16 gain_cod_unq,    /* i  : Code gain (unquantized)           */
    /*      (scaling: Q10 - exp_gcode0)       */
    Word16 *gain_cod,       /* i/o: Code gain (pre-/quantized),   Q1  */
    Word16 *qua_ener_MR122, /* o  : quantized energy error,       Q10 */
    /*      (for MR122 MA predictor update)   */
    Word16 *qua_ener,       /* o  : quantized energy error,       Q10 */
    /*      (for other MA predictor update)   */
    const Word16* qua_gain_code_ptr, /* i : ptr to read-only ptr      */
    Flag   *pOverflow       /* o  : overflow indicator                */
)
{
    const Word16 *p;
    Word16 i;
    Word16 index;
    Word16 tmp;
    Word16 one_alpha;
    Word16 exp;
    Word16 e_max;

    Word16 g2_pitch;
    Word16 g_code;
    Word16 g2_code_h;
    Word16 g2_code_l;
    Word16 d2_code_h;
    Word16 d2_code_l;
    Word16 coeff[5];
    Word16 coeff_lo[5];
    Word16 exp_coeff[5];
    Word32 L_tmp;
    Word32 L_t0;
    Word32 L_t1;
    Word32 dist_min;
    Word16 gain_code;

    /*
      Steps in calculation of the error criterion (dist):
      ---------------------------------------------------

      underlined = constant; alp = FLP value of alpha, alpha = FIP
      ----------


        ExEn = gp^2 * LtpEn + 2.0*gp*gc[i] * XC + gc[i]^2 * InnEn;
               ------------   ------         --             -----

        aExEn= alp * ExEn
             = alp*gp^2*LtpEn + 2.0*alp*gp*XC* gc[i] + alp*InnEn* gc[i]^2
               --------------   -------------          ---------

             =         t[1]   +              t[2]    +          t[3]

        dist = d1 + d2;

          d1 = (1.0 - alp) * InnEn * (gcu - gc[i])^2 = t[4]
               -------------------    ---

          d2 =        alp  * (ResEn - 2.0 * sqrt(ResEn*ExEn) + ExEn);
                      ---     -----   ---        -----

             =        alp  * (sqrt(ExEn) - sqrt(ResEn))^2
                      ---                  -----------

             =               (sqrt(aExEn) - sqrt(alp*ResEn))^2
                                            ---------------

             =               (sqrt(aExEn) -       t[0]     )^2
                                                  ----

     */

    /*
     * calculate scalings of the constant terms
     */
    gain_code = shl(*gain_cod, (10 - exp_gcode0), pOverflow);   /* Q1  -> Q11 (-ec0) */
    g2_pitch = mult(gain_pit, gain_pit, pOverflow);               /* Q14 -> Q13        */
    /* 0 < alpha <= 0.5 => 0.5 <= 1-alpha < 1, i.e one_alpha is normalized  */
    one_alpha = add_16((32767 - alpha), 1, pOverflow);   /* 32768 - alpha */


    /*  alpha <= 0.5 -> mult. by 2 to keep precision; compensate in exponent */
    L_t1 = L_mult(alpha, frac_en[1], pOverflow);
    L_t1 = L_shl(L_t1, 1, pOverflow);
    tmp = (Word16)(L_t1 >> 16);

    /* directly store in 32 bit variable because no further mult. required */
    L_t1 = L_mult(tmp, g2_pitch, pOverflow);
    exp_coeff[1] = exp_en[1] - 15;


    tmp = (Word16)(L_shl(L_mult(alpha, frac_en[2], pOverflow), 1, pOverflow) >> 16);
    coeff[2] = mult(tmp, gain_pit, pOverflow);
    exp = exp_gcode0 - 10;
    exp_coeff[2] = add_16(exp_en[2], exp, pOverflow);


    /* alpha <= 0.5 -> mult. by 2 to keep precision; compensate in exponent */
    coeff[3] = (Word16)(L_shl(L_mult(alpha, frac_en[3], pOverflow), 1, pOverflow) >> 16);
    exp = shl(exp_gcode0, 1, pOverflow) - 7;
    exp_coeff[3] = add_16(exp_en[3], exp, pOverflow);


    coeff[4] = mult(one_alpha, frac_en[3], pOverflow);
    exp_coeff[4] = add_16(exp_coeff[3], 1, pOverflow);


    L_tmp = L_mult(alpha, frac_en[0], pOverflow);
    /* sqrt_l returns normalized value and 2*exponent
       -> result = val >> (exp/2)
       exp_coeff holds 2*exponent for c[0]            */
    /* directly store in 32 bit variable because no further mult. required */
    L_t0 = sqrt_l_exp(L_tmp, &exp, pOverflow);  /* normalization included in sqrt_l_exp */
    exp += 47;
    exp_coeff[0] = exp_en[0] - exp;

    /*
     * Determine the maximum exponent occuring in the distance calculation
     * and adjust all fractions accordingly (including a safety margin)
     *
     */

    /* find max(e[1..4],e[0]+31) */
    e_max = exp_coeff[0] + 31;
    for (i = 1; i <= 4; i++)
    {
        if (exp_coeff[i] > e_max)
        {
            e_max = exp_coeff[i];
        }
    }

    /* scale c[1]         (requires no further multiplication) */
    tmp = e_max - exp_coeff[1];
    L_t1 = L_shr(L_t1, tmp, pOverflow);

    /* scale c[2..4] (used in Mpy_32_16 in the quantizer loop) */
    for (i = 2; i <= 4; i++)
    {
        tmp = e_max - exp_coeff[i];
        L_tmp = ((Word32)coeff[i] << 16);
        L_tmp = L_shr(L_tmp, tmp, pOverflow);
        L_Extract(L_tmp, &coeff[i], &coeff_lo[i], pOverflow);
    }

    /* scale c[0]         (requires no further multiplication) */
    exp = e_max - 31;              /* new exponent */
    tmp = exp - exp_coeff[0];
    L_t0 = L_shr(L_t0, shr(tmp, 1, pOverflow), pOverflow);
    /* perform correction by 1/sqrt(2) if exponent difference is odd */
    if ((tmp & 0x1) != 0)
    {
        L_Extract(L_t0, &coeff[0], &coeff_lo[0], pOverflow);
        L_t0 = Mpy_32_16(coeff[0], coeff_lo[0],
                         23170, pOverflow);                    /* 23170 Q15 = 1/sqrt(2)*/
    }

    /* search the quantizer table for the lowest value
       of the search criterion                           */
    dist_min = MAX_32;
    index = 0;
    p = &qua_gain_code_ptr[0];

    for (i = 0; i < NB_QUA_CODE; i++)
    {
        g_code = *p++;                   /* this is g_fac (Q11)  */
        p++;                             /* skip log2(g_fac)     */
        p++;                             /* skip 20*log10(g_fac) */
        g_code = mult(g_code, gcode0, pOverflow);

        /* only continue if    gc[i]            < 2.0*gc
           which is equiv. to  g_code (Q10-ec0) < gain_code (Q11-ec0) */

        if (g_code >= gain_code)
        {
            break;
        }

        L_tmp = L_mult(g_code, g_code, pOverflow);
        L_Extract(L_tmp, &g2_code_h, &g2_code_l, pOverflow);

        tmp = sub(g_code, gain_cod_unq, pOverflow);
        L_tmp = L_mult(tmp, tmp, pOverflow);
        L_Extract(L_tmp, &d2_code_h, &d2_code_l, pOverflow);

        /* t2, t3, t4 */
        L_tmp = Mac_32_16(L_t1, coeff[2], coeff_lo[2], g_code, pOverflow);
        L_tmp = Mac_32(L_tmp,    coeff[3], coeff_lo[3], g2_code_h, g2_code_l, pOverflow);

        L_tmp = sqrt_l_exp(L_tmp, &exp, pOverflow);
        L_tmp = L_shr(L_tmp, shr(exp, 1, pOverflow), pOverflow);

        /* d2 */
        tmp = pv_round(L_sub(L_tmp, L_t0, pOverflow), pOverflow);
        L_tmp = L_mult(tmp, tmp, pOverflow);

        /* dist */
        L_tmp = Mac_32(L_tmp, coeff[4], coeff_lo[4], d2_code_h, d2_code_l, pOverflow);

        /* store table index if distance measure for this
            index is lower than the minimum seen so far   */
        if (L_tmp < dist_min)
        {
            dist_min = L_tmp;
            index = i;
        }
    }

    /*------------------------------------------------------------------*
     *  read quantized gains and new values for MA predictor memories   *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~   *
     *------------------------------------------------------------------*/

    /* Read the quantized gains */
    p = &qua_gain_code_ptr[(index<<2) - index];
    g_code = *p++;
    *qua_ener_MR122 = *p++;
    *qua_ener = *p;

    /*------------------------------------------------------------------*
     *  calculate final fixed codebook gain:                            *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                            *
     *                                                                  *
     *   gc = gc0 * g                                                   *
     *------------------------------------------------------------------*/

    L_tmp = L_mult(g_code, gcode0, pOverflow);
    L_tmp = L_shr(L_tmp, 9 - exp_gcode0, pOverflow);
    *gain_cod = (Word16)(L_tmp >> 16);

    return index;
}
Exemple #2
0
/*---------------------------------------------------------------------------*
 * Function  Dec_gain                                                        *
 * ~~~~~~~~~~~~~~~~~~                                                        *
 * Decode the pitch and codebook gains                                       *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * input arguments:                                                          *
 *                                                                           *
 *   index      :Quantization index                                          *
 *   code[]     :Innovative code vector                                      *
 *   L_subfr    :Subframe size                                               *
 *   bfi        :Bad frame indicator                                         *
 *                                                                           *
 * output arguments:                                                         *
 *                                                                           *
 *   gain_pit   :Quantized pitch gain                                        *
 *   gain_cod   :Quantized codebook gain                                     *
 *                                                                           *
 *---------------------------------------------------------------------------*/
void WebRtcG729fix_Dec_gain(
   Decod_ld8a_state *st,
   int16_t index,        /* (i)     :Index of quantization.         */
   int16_t code[],       /* (i) Q13 :Innovative vector.             */
   int16_t L_subfr,      /* (i)     :Subframe length.               */
   int16_t bfi,          /* (i)     :Bad frame indicator            */
   int16_t *gain_pit,    /* (o) Q14 :Pitch gain.                    */
   int16_t *gain_cod     /* (o) Q1  :Code gain.                     */
)
{
   int16_t  index1, index2, tmp;
   int16_t  gcode0, exp_gcode0;
   int32_t  L_gbk12, L_acc, L_accb;

   /*-------------- Case of erasure. ---------------*/

   if(bfi != 0){
      *gain_pit = mult( *gain_pit, 29491 );      /* *0.9 in Q15 */
      if (*gain_pit > 29491) *gain_pit = 29491;
      *gain_cod = mult( *gain_cod, 32111 );      /* *0.98 in Q15 */

     /*----------------------------------------------*
      * update table of past quantized energies      *
      *                              (frame erasure) *
      *----------------------------------------------*/
      WebRtcG729fix_Gain_update_erasure(st->past_qua_en);

      return;
   }

   /*-------------- Decode pitch gain ---------------*/

   index1 = WebRtcG729fix_imap1[ shr(index,NCODE2_B) ] ;
   index2 = WebRtcG729fix_imap2[ index & (NCODE2-1) ] ;
   *gain_pit = WebRtcSpl_AddSatW16( WebRtcG729fix_gbk1[index1][0], WebRtcG729fix_gbk2[index2][0] );

   /*-------------- Decode codebook gain ---------------*/

  /*---------------------------------------------------*
   *-  energy due to innovation                       -*
   *-  predicted energy                               -*
   *-  predicted codebook gain => gcode0[exp_gcode0]  -*
   *---------------------------------------------------*/

   WebRtcG729fix_Gain_predict(st->past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );

  /*-----------------------------------------------------------------*
   * *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0;      *
   *-----------------------------------------------------------------*/

   L_acc = L_deposit_l( WebRtcG729fix_gbk1[index1][1] );
   L_accb = L_deposit_l( WebRtcG729fix_gbk2[index2][1] );
   L_gbk12 = WebRtcSpl_AddSatW32( L_acc, L_accb );                       /* Q13 */
   tmp = extract_l( L_shr( L_gbk12,1 ) );                  /* Q12 */
   L_acc = L_mult(tmp, gcode0);             /* Q[exp_gcode0+12+1] */

   L_acc = L_shl(L_acc, WebRtcSpl_AddSatW16( negate(exp_gcode0),(-12-1+1+16) ));
   *gain_cod = extract_h( L_acc );                          /* Q1 */

  /*----------------------------------------------*
   * update table of past quantized energies      *
   *----------------------------------------------*/
   WebRtcG729fix_Gain_update(st->past_qua_en, L_gbk12 );

   return;

}
Exemple #3
0
void perc_var (
  Word16 *gamma1, /* Bandwidth expansion parameter */
  Word16 *gamma2, /* Bandwidth expansion parameter */
  Word16 *LsfInt, /* Interpolated LSP vector : 1st subframe */
  Word16 *LsfNew, /* New LSP vector : 2nd subframe */
  Word16 *r_c     /* Reflection coefficients */
)
{

  Word32   L_temp;
  Word16   cur_rc;                    /* Q11 */
  Word16   Lar[4];                    /* Q11 */
  Word16  *LarNew;                    /* Q11 */
  Word16  *Lsf;                       /* Q15 */
  Word16   CritLar0, CritLar1;        /* Q11 */
  Word16   temp;
  Word16   d_min;                     /* Q10 */
  Word16   i, k;



  for (k=0; k<M; k++) {
    LsfInt[k] = shl(LsfInt[k], 1);
    LsfNew[k] = shl(LsfNew[k], 1);
  }


  LarNew = &Lar[2];
  /* ---------------------------------------- */
  /* Reflection coefficients ---> Lar         */
  /* Lar(i) = log10( (1+rc) / (1-rc) )        */
  /* Approximated by                          */
  /* x <= SEG1            y = x               */
  /* SEG1 < x <= SEG2     y = A1 x - B1_L     */
  /* SEG2 < x <= SEG3     y = A2 x - B2_L     */
  /* x > SEG3             y = A3 x - B3_L     */
  /* ---------------------------------------- */
  for (i=0; i<2; i++) {

    cur_rc = abs_s(r_c[i]);
    cur_rc = shr(cur_rc, 4);

    if (sub(cur_rc ,SEG1)<= 0) {
        LarNew[i] = cur_rc;
    }
    else {
      if (sub(cur_rc,SEG2)<= 0) {
        cur_rc = shr(cur_rc, 1);
        L_temp = L_mult(cur_rc, A1);
        L_temp = L_sub(L_temp, L_B1);
        L_temp = L_shr(L_temp, 11);
        LarNew[i] = extract_l(L_temp);
      }
      else {
        if (sub(cur_rc ,SEG3)<= 0) {
          cur_rc = shr(cur_rc, 1);
          L_temp = L_mult(cur_rc, A2);
          L_temp = L_sub(L_temp, L_B2);
          L_temp = L_shr(L_temp, 11);
          LarNew[i] = extract_l(L_temp);
        }
        else {
          cur_rc = shr(cur_rc, 1);
          L_temp = L_mult(cur_rc, A3);
          L_temp = L_sub(L_temp, L_B3);
          L_temp = L_shr(L_temp, 11);
          LarNew[i] = extract_l(L_temp);
        }
      }
    }
    if (r_c[i] < 0) {
        LarNew[i] = sub(0, LarNew[i]);

    }
  }

  /* Interpolation of Lar for the 1st subframe */

  temp = add(LarNew[0], LarOld[0]);
  Lar[0] = shr(temp, 1);
  LarOld[0] = LarNew[0];
  temp = add(LarNew[1], LarOld[1]);
  Lar[1] = shr(temp, 1);
  LarOld[1] = LarNew[1];

  for (k=0; k<2; k++) { /* LOOP : gamma2 for 1st to 2nd subframes */

      /* ---------------------------------------------------------- */
      /* First criterion based on the first two Lars                */
      /* smooth == 1  ==>  gamma2 can vary from 0.4 to 0.7          */
      /* smooth == 0  ==>  gamma2 is set to 0.6                     */
      /*                                                            */
      /* Double threshold + hysteresis :                            */
      /* if smooth = 1                                              */
      /*  if (CritLar0 < THRESH_L1) and (CritLar1 > THRESH_H1)      */
      /*                                                 smooth = 0 */
      /* if smooth = 0                                              */
      /*  if (CritLar0 > THRESH_L2) or (CritLar1 < THRESH_H2)       */
      /*                                                 smooth = 1 */
      /* ---------------------------------------------------------- */

      CritLar0 = Lar[2*k];
      CritLar1 = Lar[2*k+1];

      if (smooth != 0) {
        if ((sub(CritLar0,THRESH_L1)<0)&&( sub(CritLar1,THRESH_H1)>0)) {
            smooth = 0;
        }
      }
      else {
        if ( (sub(CritLar0 ,THRESH_L2)>0) || (sub(CritLar1,THRESH_H2) <0) ) {
            smooth = 1;
        }

      }

    if (smooth == 0) {
      /* ------------------------------------------------------ */
      /* Second criterion based on the minimum distance between */
      /*                two successives LSPs                    */
      /*                                                        */
      /*           gamma2[k] = -6.0 * pi * d_min + 1.0          */
      /*                                                        */
      /*       with Lsfs normalized range 0.0 <= val <= 1.0     */
      /* ------------------------------------------------------ */
      gamma1[k] = GAMMA1_0;
      if (k == 0) {
        Lsf = LsfInt;
      }
      else {
        Lsf = LsfNew;
      }
      d_min = sub(Lsf[1], Lsf[0]);
      for (i=1; i<M-1; i++) {
        temp = sub(Lsf[i+1],Lsf[i]);
        if (sub(temp,d_min)<0) {
            d_min = temp;
        }
      }
      temp = mult(ALPHA, d_min);
      temp = sub(BETA, temp);
      temp = shl(temp, 5);
      gamma2[k] = temp;

      if (sub(gamma2[k] , GAMMA2_0_H)>0) {
        gamma2[k] = GAMMA2_0_H;
      }
      if (sub(gamma2[k] ,GAMMA2_0_L)<0) {
        gamma2[k] = GAMMA2_0_L;
      }

    }
    else {
      gamma1[k] = GAMMA1_1;
      gamma2[k] = GAMMA2_1;
    }
  }
  return;
}
Exemple #4
0
/*-----------------------------------------------------------------*
 * Functions vad                                                   *
 *           ~~~                                                   *
 * Input:                                                          *
 *   rc            : reflection coefficient                        *
 *   lsf[]         : unquantized lsf vector                        *
 *   r_h[]         : upper 16-bits of the autocorrelation vector   *
 *   r_l[]         : lower 16-bits of the autocorrelation vector   *
 *   exp_R0        : exponent of the autocorrelation vector        *
 *   sigpp[]       : preprocessed input signal                     *
 *   frm_count     : frame counter                                 *
 *   prev_marker   : VAD decision of the last frame                *
 *   pprev_marker  : VAD decision of the frame before last frame   *
 *                                                                 *
 * Output:                                                         *
 *                                                                 *
 *   marker        : VAD decision of the current frame             * 
 *                                                                 *
 *-----------------------------------------------------------------*/
void vad(
         Word16 rc,
         Word16 *lsf, 
         Word16 *r_h,
         Word16 *r_l, 
         Word16 exp_R0,
         Word16 *sigpp,
         Word16 frm_count,
         Word16 prev_marker,
         Word16 pprev_marker,
         Word16 *marker)
{
 /* scalar */
  Word32 acc0;
  Word16 i, j, exp, frac;
  Word16 ENERGY, ENERGY_low, SD, ZC, dSE, dSLE, dSZC;
  Word16 COEF, C_COEF, COEFZC, C_COEFZC, COEFSD, C_COEFSD;

  /* compute the frame energy */
  acc0 = L_Comp(r_h[0], r_l[0]);
  Log2(acc0, &exp, &frac);
  acc0 = Mpy_32_16(exp, frac, 9864);
  i = sub(exp_R0, 1);  
  i = sub(i, 1);
  acc0 = L_mac(acc0, 9864, i);
  acc0 = L_shl(acc0, 11);
  ENERGY = extract_h(acc0);
  ENERGY = sub(ENERGY, 4875);

  /* compute the low band energy */
  acc0 = 0;
  for (i=1; i<=NP; i++)
    acc0 = L_mac(acc0, r_h[i], lbf_corr[i]);
  acc0 = L_shl(acc0, 1);
  acc0 = L_mac(acc0, r_h[0], lbf_corr[0]);
  Log2(acc0, &exp, &frac);
  acc0 = Mpy_32_16(exp, frac, 9864);
  i = sub(exp_R0, 1);  
  i = sub(i, 1);
  acc0 = L_mac(acc0, 9864, i);
  acc0 = L_shl(acc0, 11);
  ENERGY_low = extract_h(acc0);
  ENERGY_low = sub(ENERGY_low, 4875);
  
  /* compute SD */
  acc0 = 0;
  for (i=0; i<M; i++){
    j = sub(lsf[i], MeanLSF[i]);
    acc0 = L_mac(acc0, j, j);
  }
  SD = extract_h(acc0);      /* Q15 */
  
  /* compute # zero crossing */
  ZC = 0;
  for (i=ZC_START+1; i<=ZC_END; i++)
    if (mult(sigpp[i-1], sigpp[i]) < 0)
      ZC = add(ZC, 410);     /* Q15 */

  /* Initialize and update Mins */
  if(sub(frm_count, 129) < 0){
    if (sub(ENERGY, Min) < 0){
      Min = ENERGY;
      Prev_Min = ENERGY;
    }
    
    if((frm_count & 0x0007) == 0){
      i = sub(shr(frm_count,3),1);
      Min_buffer[i] = Min;
      Min = MAX_16;
    }
  }

  if((frm_count & 0x0007) == 0){
    Prev_Min = Min_buffer[0];
    for (i=1; i<16; i++){
      if (sub(Min_buffer[i], Prev_Min) < 0)
        Prev_Min = Min_buffer[i];
    }
  }
  
  if(sub(frm_count, 129) >= 0){
    if(((frm_count & 0x0007) ^ (0x0001)) == 0){
      Min = Prev_Min;
      Next_Min = MAX_16;
    }
    if (sub(ENERGY, Min) < 0)
      Min = ENERGY;
    if (sub(ENERGY, Next_Min) < 0)
      Next_Min = ENERGY;
    
    if((frm_count & 0x0007) == 0){
      for (i=0; i<15; i++)
        Min_buffer[i] = Min_buffer[i+1]; 
      Min_buffer[15] = Next_Min; 
      Prev_Min = Min_buffer[0];
      for (i=1; i<16; i++) 
        if (sub(Min_buffer[i], Prev_Min) < 0)
          Prev_Min = Min_buffer[i];
    }
    
  }

  if (sub(frm_count, INIT_FRAME) <= 0)
    if(sub(ENERGY, 3072) < 0){
      *marker = NOISE;
      less_count++;
    }
    else{
      *marker = VOICE;
      acc0 = L_deposit_h(MeanE);
      acc0 = L_mac(acc0, ENERGY, 1024);
      MeanE = extract_h(acc0);
      acc0 = L_deposit_h(MeanSZC);
      acc0 = L_mac(acc0, ZC, 1024);
      MeanSZC = extract_h(acc0);
      for (i=0; i<M; i++){
        acc0 = L_deposit_h(MeanLSF[i]);
        acc0 = L_mac(acc0, lsf[i], 1024);
        MeanLSF[i] = extract_h(acc0);
      }
    }
  
  if (sub(frm_count, INIT_FRAME) >= 0){
    if (sub(frm_count, INIT_FRAME) == 0){
      acc0 = L_mult(MeanE, factor_fx[less_count]);
      acc0 = L_shl(acc0, shift_fx[less_count]);
      MeanE = extract_h(acc0);

      acc0 = L_mult(MeanSZC, factor_fx[less_count]);
      acc0 = L_shl(acc0, shift_fx[less_count]);
      MeanSZC = extract_h(acc0);

      for (i=0; i<M; i++){
        acc0 = L_mult(MeanLSF[i], factor_fx[less_count]);
        acc0 = L_shl(acc0, shift_fx[less_count]);
        MeanLSF[i] = extract_h(acc0);
      }

      MeanSE = sub(MeanE, 2048);   /* Q11 */
      MeanSLE = sub(MeanE, 2458);  /* Q11 */
    }

    dSE = sub(MeanSE, ENERGY);
    dSLE = sub(MeanSLE, ENERGY_low);
    dSZC = sub(MeanSZC, ZC);

    if(sub(ENERGY, 3072) < 0)
      *marker = NOISE;
    else 
      *marker = MakeDec(dSLE, dSE, SD, dSZC);

    v_flag = 0;
    if((prev_marker==VOICE) && (*marker==NOISE) && (add(dSE,410) < 0) 
       && (sub(ENERGY, 3072)>0)){
      *marker = VOICE;
      v_flag = 1;
    }

    if(flag == 1){
      if((pprev_marker == VOICE) && 
         (prev_marker == VOICE) && 
         (*marker == NOISE) && 
         (sub(abs_s(sub(prev_energy,ENERGY)), 614) <= 0)){
        count_ext++;
        *marker = VOICE;
        v_flag = 1;
        if(sub(count_ext, 4) <= 0)
          flag=1;
        else{
          count_ext=0;
          flag=0;
        }
      }
    }
    else
      flag=1;
    
    if(*marker == NOISE)
      count_sil++;

    if((*marker == VOICE) && (sub(count_sil, 10) > 0) && 
       (sub(sub(ENERGY,prev_energy), 614) <= 0)){
      *marker = NOISE;
      count_sil=0;
    }

    if(*marker == VOICE)
      count_sil=0;

    if ((sub(sub(ENERGY, 614), MeanSE) < 0) && (sub(frm_count, 128) > 0)
        && (!v_flag) && (sub(rc, 19661) < 0))
      *marker = NOISE;

    if ((sub(sub(ENERGY,614),MeanSE) < 0) && (sub(rc, 24576) < 0)
        && (sub(SD, 83) < 0)){ 
      count_update++;
      if (sub(count_update, INIT_COUNT) < 0){
        COEF = 24576;
        C_COEF = 8192;
        COEFZC = 26214;
        C_COEFZC = 6554;
        COEFSD = 19661;
        C_COEFSD = 13017;
      } 
      else
        if (sub(count_update, INIT_COUNT+10) < 0){
          COEF = 31130;
          C_COEF = 1638;
          COEFZC = 30147;
          C_COEFZC = 2621;
          COEFSD = 21299;
          C_COEFSD = 11469;
        }
        else
          if (sub(count_update, INIT_COUNT+20) < 0){
            COEF = 31785;
            C_COEF = 983;
            COEFZC = 30802;
            C_COEFZC = 1966;
            COEFSD = 22938;
            C_COEFSD = 9830;
          }
          else
            if (sub(count_update, INIT_COUNT+30) < 0){
              COEF = 32440;
              C_COEF = 328;
              COEFZC = 31457;
              C_COEFZC = 1311;
              COEFSD = 24576;
              C_COEFSD = 8192;
            }
            else
              if (sub(count_update, INIT_COUNT+40) < 0){
                COEF = 32604;
                C_COEF = 164;
                COEFZC = 32440;
                C_COEFZC = 328;
                COEFSD = 24576;
                C_COEFSD = 8192;
              }
              else{
                COEF = 32604;
                C_COEF = 164;
                COEFZC = 32702;
                C_COEFZC = 66;
                COEFSD = 24576;
                C_COEFSD = 8192;
              }
      

      /* compute MeanSE */
      acc0 = L_mult(COEF, MeanSE);
      acc0 = L_mac(acc0, C_COEF, ENERGY);
      MeanSE = extract_h(acc0);

      /* compute MeanSLE */
      acc0 = L_mult(COEF, MeanSLE);
      acc0 = L_mac(acc0, C_COEF, ENERGY_low);
      MeanSLE = extract_h(acc0);

      /* compute MeanSZC */
      acc0 = L_mult(COEFZC, MeanSZC);
      acc0 = L_mac(acc0, C_COEFZC, ZC);
      MeanSZC = extract_h(acc0);
      
      /* compute MeanLSF */
      for (i=0; i<M; i++){
        acc0 = L_mult(COEFSD, MeanLSF[i]);
        acc0 = L_mac(acc0, C_COEFSD, lsf[i]);
        MeanLSF[i] = extract_h(acc0);
      }
    }

    if((sub(frm_count, 128) > 0) && (((sub(MeanSE,Min) < 0) &&
                        (sub(SD, 83) < 0)) || (sub(MeanSE,Min) > 2048))){
      MeanSE = Min;
      count_update = 0;
    }
  }

  prev_energy = ENERGY;

}
Exemple #5
0
inline ivect mult(ivect v, imat M) {
    return matr2vec(mult(vec2matr(v), M), 0);
}
Exemple #6
0
static int three_register_execute(UM_machine machine,
                                  Um_instruction instruction)
{
        unsigned op = Bitpack_getu(instruction, BITS_PER_OP_CODE, 28);
        unsigned A = Bitpack_getu(instruction, BITS_PER_REG_NUM, 6);
        unsigned B = Bitpack_getu(instruction, BITS_PER_REG_NUM, 3);
        unsigned C = Bitpack_getu(instruction, BITS_PER_REG_NUM, 0);

        switch(op){
                case 0: //
                        movc(machine, A, B, C);
                        return 1;

                case 1:
                        segl(machine, A, B, C);
                        return 1;
                        
                case 2:
                        segs(machine, A, B, C);
                        return 1;

                case 3:
                        add(machine, A, B, C); 
                        return 1;

                case 4:
                        mult(machine, A, B, C);
                        return 1;

                case 5:
                        divi(machine, A, B, C);
                        return 1;

                case 6:
                        nand(machine, A, B, C);
                        return 1;

                case 7:
                        return 0;

                case 8:
                        map(machine, B, C);
                        return 1;

                case 9: 
                        unmap(machine, C);
                        return 1;

                case 10:
                        out(machine, C);
                        return 1;

                case 11:
                        in(machine, C);
                        return 1;

                case 12:
                        loadp(machine, B, C);
                        return 1;
                default:
                        return 0;
        }
}
Exemple #7
0
std::uint16_t VirtualCPU::executeInstructionAtAddress(std::uint16_t address) {
  debugger_.pc_  = address; //HACK FOR NOW
    switch (memoryController_.readAtAddress(address)) {
        case 0:
            return address + halt();
        case 1:
            return address + set(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2));
        case 2:
            return address + push(memoryController_.readAtAddress(address + 1));
        case 3:
            return address + pop(memoryController_.readAtAddress(address + 1));
        case 4:
            return address +
                    eq(memoryController_.readAtAddress(address + 1),
                       memoryController_.readAtAddress(address + 2),
                       memoryController_.readAtAddress(address + 3));
        case 5:
            return address + gt(memoryController_.readAtAddress(address + 1),
                                memoryController_.readAtAddress(address +2),
                                memoryController_.readAtAddress(address + 3));
        case 6:
            return jump(memoryController_.readAtAddress(address + 1), address);
        case 7:
            return jt(memoryController_.readAtAddress(address + 1),
                                memoryController_.readAtAddress(address + 2),
                                address);
        case 8:
            return jf(memoryController_.readAtAddress(address + 1),
                                memoryController_.readAtAddress(address + 2),
                                address);
        case 9:
            return address + add(memoryController_.readAtAddress(address + 1),
                                 memoryController_.readAtAddress(address + 2),
                                 memoryController_.readAtAddress(address + 3));
        case 10:
            return address + mult(memoryController_.readAtAddress(address + 1),
                                 memoryController_.readAtAddress(address + 2),
                                 memoryController_.readAtAddress(address + 3));
        case 11:
            return address + mod(memoryController_.readAtAddress(address + 1),
                                  memoryController_.readAtAddress(address + 2),
                                  memoryController_.readAtAddress(address + 3));
        case 12:
            return address + and_i(memoryController_.readAtAddress(address + 1),
                                 memoryController_.readAtAddress(address + 2),
                                 memoryController_.readAtAddress(address + 3));
        case 13:
            return address + or_i(memoryController_.readAtAddress(address + 1),
                                 memoryController_.readAtAddress(address + 2),
                                 memoryController_.readAtAddress(address + 3));
        case 14:
            return address + not_i(memoryController_.readAtAddress(address + 1),
                                 memoryController_.readAtAddress(address + 2));
        case 15:
            return address + rmem(memoryController_.readAtAddress(address + 1),
                                 memoryController_.readAtAddress(address + 2));
        case 16:
            return address + wmem(memoryController_.readAtAddress(address + 1),
                                  memoryController_.readAtAddress(address + 2));
        case 17:
            return call(memoryController_.readAtAddress(address + 1), address);
        case 18:
            return ret();
        case 19:
            return address + out(memoryController_.readAtAddress(address + 1));
        case 21:
            return address + noop();
        case 20:
            return address + in(memoryController_.readAtAddress(address + 1));
        default:
            std::cout << "CPU ERROR illegal op code: " << memoryController_.readAtAddress(address) << std::endl;
            return address + 1;
    }
}
Exemple #8
0
void Dec_lag3(
  Word16 index,       /* input : received pitch index           */
  Word16 pit_min,     /* input : minimum pitch lag              */
  Word16 pit_max,     /* input : maximum pitch lag              */
  Word16 i_subfr,     /* input : subframe flag                  */
  Word16 *T0,         /* output: integer part of pitch lag      */
  Word16 *T0_frac     /* output: fractional part of pitch lag   */
)
{
  Word16 i;
  Word16 T0_min, T0_max;

  if (i_subfr == 0)                  /* if 1st subframe */
  {
    if (sub(index, 197) < 0)
    {
      /* *T0 = (index+2)/3 + 19 */

      *T0 = add(mult(add(index, 2), 10923), 19);

      /* *T0_frac = index - *T0*3 + 58 */

      i = add(add(*T0, *T0), *T0);
      *T0_frac = add(sub(index, i), 58);
    }
    else
    {
      *T0 = sub(index, 112);
      *T0_frac = 0;
    }

  }

  else  /* second subframe */
  {
    /* find T0_min and T0_max for 2nd subframe */

    T0_min = sub(*T0, 5);
    if (sub(T0_min, pit_min) < 0)
    {
      T0_min = pit_min;
    }

    T0_max = add(T0_min, 9);
    if (sub(T0_max, pit_max) > 0)
    {
      T0_max = pit_max;
      T0_min = sub(T0_max, 9);
    }

    /* i = (index+2)/3 - 1 */
    /* *T0 = i + t0_min;    */

    i = sub(mult(add(index, 2), 10923), 1);
    *T0 = add(i, T0_min);

    /* t0_frac = index - 2 - i*3; */

    i = add(add(i, i), i);
    *T0_frac = sub(sub(index, 2), i);
  }

  return;
}
Exemple #9
0
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
		inline void mult_left(const quaternion& left) {
			mult(left, *this);
		}
Exemple #10
0
 DiagonalMatrix &DM::multT(const DiagonalMatrix &S, DiagonalMatrix &ans,
                           double scal) const {
   return mult(S, ans, scal);
 }
Exemple #11
0
void Coder_ld8a(
      g729a_encoder_state *state,
     Word16 ana[]       /* output  : Analysis parameters */
)
{

  /* LPC analysis */

  Word16 Aq_t[(MP1)*2];         /* A(z)   quantized for the 2 subframes */
  Word16 Ap_t[(MP1)*2];         /* A(z/gamma)       for the 2 subframes */
  Word16 *Aq, *Ap;              /* Pointer on Aq_t and Ap_t             */

  /* Other vectors */

  Word16 h1[L_SUBFR];            /* Impulse response h1[]              */
  Word16 xn[L_SUBFR];            /* Target vector for pitch search     */
  Word16 xn2[L_SUBFR];           /* Target vector for codebook search  */
  Word16 code[L_SUBFR];          /* Fixed codebook excitation          */
  Word16 y1[L_SUBFR];            /* Filtered adaptive excitation       */
  Word16 y2[L_SUBFR];            /* Filtered fixed codebook excitation */
  Word16 g_coeff[4];             /* Correlations between xn & y1       */

  Word16 g_coeff_cs[5];
  Word16 exp_g_coeff_cs[5];      /* Correlations between xn, y1, & y2
                                     <y1,y1>, -2<xn,y1>,
                                          <y2,y2>, -2<xn,y2>, 2<y1,y2> */

  /* Scalars */

  Word16 i, j, k, i_subfr;
  Word16 T_op, T0, T0_min, T0_max, T0_frac;
  Word16 gain_pit, gain_code, index;
  Word16 temp, taming;
  Word32 L_temp;

/*------------------------------------------------------------------------*
 *  - Perform LPC analysis:                                               *
 *       * autocorrelation + lag windowing                                *
 *       * Levinson-durbin algorithm to find a[]                          *
 *       * convert a[] to lsp[]                                           *
 *       * quantize and code the LSPs                                     *
 *       * find the interpolated LSPs and convert to a[] for the 2        *
 *         subframes (both quantized and unquantized)                     *
 *------------------------------------------------------------------------*/
  {
     /* Temporary vectors */
    Word16 r_l[MP1], r_h[MP1];       /* Autocorrelations low and hi          */
    Word16 rc[M];                    /* Reflection coefficients.             */
    Word16 lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe                 */

    /* LP analysis */

    Autocorr(state->p_window, M, r_h, r_l);              /* Autocorrelations */
    Lag_window(M, r_h, r_l);                      /* Lag windowing    */
    Levinson(r_h, r_l, Ap_t, rc);                 /* Levinson Durbin  */
    Az_lsp(Ap_t, lsp_new, state->lsp_old);               /* From A(z) to lsp */

    /* LSP quantization */

    Qua_lsp(state, lsp_new, lsp_new_q, ana);
    ana += 2;                         /* Advance analysis parameters pointer */

    /*--------------------------------------------------------------------*
     * Find interpolated LPC parameters in all subframes                  *
     * The interpolated parameters are in array Aq_t[].                   *
     *--------------------------------------------------------------------*/

    Int_qlpc(state->lsp_old_q, lsp_new_q, Aq_t);

    /* Compute A(z/gamma) */

    Weight_Az(&Aq_t[0],   GAMMA1, M, &Ap_t[0]);
    Weight_Az(&Aq_t[MP1], GAMMA1, M, &Ap_t[MP1]);

    /* update the LSPs for the next frame */

    Copy(lsp_new,   state->lsp_old,   M);
    Copy(lsp_new_q, state->lsp_old_q, M);
  }

 /*----------------------------------------------------------------------*
  * - Find the weighted input speech w_sp[] for the whole speech frame   *
  * - Find the open-loop pitch delay                                     *
  *----------------------------------------------------------------------*/

  Residu(&Aq_t[0], &(state->speech[0]), &(state->exc[0]), L_SUBFR);
  Residu(&Aq_t[MP1], &(state->speech[L_SUBFR]), &(state->exc[L_SUBFR]), L_SUBFR);

  {
    Word16 Ap1[MP1];

    Ap = Ap_t;
    Ap1[0] = 4096;
    for(i=1; i<=M; i++)    /* Ap1[i] = Ap[i] - 0.7 * Ap[i-1]; */
       Ap1[i] = sub(Ap[i], mult(Ap[i-1], 22938));
    Syn_filt(Ap1, &(state->exc[0]), &(state->wsp[0]), L_SUBFR, state->mem_w, 1);

    Ap += MP1;
    for(i=1; i<=M; i++)    /* Ap1[i] = Ap[i] - 0.7 * Ap[i-1]; */
       Ap1[i] = sub(Ap[i], mult(Ap[i-1], 22938));
    Syn_filt(Ap1, &(state->exc[L_SUBFR]), &(state->wsp[L_SUBFR]), L_SUBFR, state->mem_w, 1);
  }

  /* Find open loop pitch lag */

  T_op = Pitch_ol_fast(state->wsp, PIT_MAX, L_FRAME);

  /* Range for closed loop pitch search in 1st subframe */

  T0_min = T_op - 3;
  T0_max = T0_min + 6;
  if (T0_min < PIT_MIN)
  {
    T0_min = PIT_MIN;
    T0_max = PIT_MIN + 6;
  }
  else if (T0_max > PIT_MAX)
  {
     T0_max = PIT_MAX;
     T0_min = PIT_MAX - 6;
  }

 /*------------------------------------------------------------------------*
  *          Loop for every subframe in the analysis frame                 *
  *------------------------------------------------------------------------*
  *  To find the pitch and innovation parameters. The subframe size is     *
  *  L_SUBFR and the loop is repeated 2 times.                             *
  *     - find the weighted LPC coefficients                               *
  *     - find the LPC residual signal res[]                               *
  *     - compute the target signal for pitch search                       *
  *     - compute impulse response of weighted synthesis filter (h1[])     *
  *     - find the closed-loop pitch parameters                            *
  *     - encode the pitch delay                                           *
  *     - find target vector for codebook search                           *
  *     - codebook search                                                  *
  *     - VQ of pitch and codebook gains                                   *
  *     - update states of weighting filter                                *
  *------------------------------------------------------------------------*/

  Aq = Aq_t;    /* pointer to interpolated quantized LPC parameters */
  Ap = Ap_t;    /* pointer to weighted LPC coefficients             */

  for (i_subfr = 0;  i_subfr < L_FRAME; i_subfr += L_SUBFR)
  {

    /*---------------------------------------------------------------*
     * Compute impulse response, h1[], of weighted synthesis filter  *
     *---------------------------------------------------------------*/

    h1[0] = 4096;
    Set_zero(&h1[1], L_SUBFR-1);
    Syn_filt(Ap, h1, h1, L_SUBFR, &h1[1], 0);

   /*----------------------------------------------------------------------*
    *  Find the target vector for pitch search:                            *
    *----------------------------------------------------------------------*/

    Syn_filt(Ap, &(state->exc[i_subfr]), xn, L_SUBFR, state->mem_w0, 0);

    /*---------------------------------------------------------------------*
     *                 Closed-loop fractional pitch search                 *
     *---------------------------------------------------------------------*/

    T0 = Pitch_fr3_fast(&(state->exc[i_subfr]), xn, h1, L_SUBFR, T0_min, T0_max,
                    i_subfr, &T0_frac);

    index = Enc_lag3(T0, T0_frac, &T0_min, &T0_max,PIT_MIN,PIT_MAX,i_subfr);

    *ana++ = index;

    if (i_subfr == 0) {
      *ana++ = Parity_Pitch(index);
    }

   /*-----------------------------------------------------------------*
    *   - find filtered pitch exc                                     *
    *   - compute pitch gain and limit between 0 and 1.2              *
    *   - update target vector for codebook search                    *
    *-----------------------------------------------------------------*/

    Syn_filt(Ap, &(state->exc[i_subfr]), y1, L_SUBFR, state->mem_zero, 0);

    gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR);

    /* clip pitch gain if taming is necessary */

    taming = test_err(state, T0, T0_frac);

    if( taming == 1){
      if (gain_pit > GPCLIP) {
        gain_pit = GPCLIP;
      }
    }

    /* xn2[i]   = xn[i] - y1[i] * gain_pit  */

    for (i = 0; i < L_SUBFR; i++)
    {
      //L_temp = L_mult(y1[i], gain_pit);
      //L_temp = L_shl(L_temp, 1);               /* gain_pit in Q14 */
      L_temp = ((Word32)y1[i] * gain_pit) << 2;
      xn2[i] = sub(xn[i], extract_h(L_temp));
    }


   /*-----------------------------------------------------*
    * - Innovative codebook search.                       *
    *-----------------------------------------------------*/

    index = ACELP_Code_A(xn2, h1, T0, state->sharp, code, y2, &i);

    *ana++ = index;        /* Positions index */
    *ana++ = i;            /* Signs index     */


   /*-----------------------------------------------------*
    * - Quantization of gains.                            *
    *-----------------------------------------------------*/

    g_coeff_cs[0]     = g_coeff[0];            /* <y1,y1> */
    exp_g_coeff_cs[0] = negate(g_coeff[1]);    /* Q-Format:XXX -> JPN */
    g_coeff_cs[1]     = negate(g_coeff[2]);    /* (xn,y1) -> -2<xn,y1> */
    exp_g_coeff_cs[1] = negate(add(g_coeff[3], 1)); /* Q-Format:XXX -> JPN */

    Corr_xy2( xn, y1, y2, g_coeff_cs, exp_g_coeff_cs );  /* Q0 Q0 Q12 ^Qx ^Q0 */
                         /* g_coeff_cs[3]:exp_g_coeff_cs[3] = <y2,y2>   */
                         /* g_coeff_cs[4]:exp_g_coeff_cs[4] = -2<xn,y2> */
                         /* g_coeff_cs[5]:exp_g_coeff_cs[5] = 2<y1,y2>  */

    *ana++ = Qua_gain(code, g_coeff_cs, exp_g_coeff_cs,
                         L_SUBFR, &gain_pit, &gain_code, taming);


   /*------------------------------------------------------------*
    * - Update pitch sharpening "sharp" with quantized gain_pit  *
    *------------------------------------------------------------*/

    state->sharp = gain_pit;
    if (state->sharp > SHARPMAX)      { state->sharp = SHARPMAX;         }
    else if (state->sharp < SHARPMIN) { state->sharp = SHARPMIN;         }

   /*------------------------------------------------------*
    * - Find the total excitation                          *
    * - update filters memories for finding the target     *
    *   vector in the next subframe                        *
    *------------------------------------------------------*/

    for (i = 0; i < L_SUBFR;  i++)
    {
      /* exc[i] = gain_pit*exc[i] + gain_code*code[i]; */
      /* exc[i]  in Q0   gain_pit in Q14               */
      /* code[i] in Q13  gain_cod in Q1                */

      //L_temp = L_mult(exc[i+i_subfr], gain_pit);
      //L_temp = L_mac(L_temp, code[i], gain_code);
      //L_temp = L_shl(L_temp, 1);
      L_temp = (Word32)(state->exc[i+i_subfr]) * (Word32)gain_pit +
               (Word32)code[i] * (Word32)gain_code;
      L_temp <<= 2;
      state->exc[i+i_subfr] = g_round(L_temp);
    }

    update_exc_err(state, gain_pit, T0);

    for (i = L_SUBFR-M, j = 0; i < L_SUBFR; i++, j++)
    {
      temp       = ((Word32)y1[i] * (Word32)gain_pit)  >> 14;
      k          = ((Word32)y2[i] * (Word32)gain_code) >> 13;
      state->mem_w0[j]  = sub(xn[i], add(temp, k));
    }

    Aq += MP1;           /* interpolated LPC parameters for next subframe */
    Ap += MP1;

  }

 /*--------------------------------------------------*
  * Update signal for next frame.                    *
  * -> shift to the left by L_FRAME:                 *
  *     speech[], wsp[] and  exc[]                   *
  *--------------------------------------------------*/

  Copy(&(state->old_speech[L_FRAME]), &(state->old_speech[0]), L_TOTAL-L_FRAME);
  Copy(&(state->old_wsp[L_FRAME]), &(state->old_wsp[0]), PIT_MAX);
  Copy(&(state->old_exc[L_FRAME]), &(state->old_exc[0]), PIT_MAX+L_INTERPOL);
}
Exemple #12
0
Fichier : app.c Projet : youka2/fx
void loop(void)
{
	const float spd1=30.0f*dt();
	const float spd2=15.0f*dt();

	float r=0.0f;
	float dc=0.0f, ds=0.0f;
	float tc=0.0f, ts=0.0f;

	/*viewport(0,0,nsw,nsh);*/
	use_basic();
	send_lpview(pview);

	if(mm() || mw()!=0.0f)
	{
		if(mm())
		{
			dir+=mx()*spd1;
			if(dir<-0.0f)
			{
				dir+=360.0f;
				ndir+=360.0f;
			}
			else if(dir>360.0f)
			{
				dir-=360.0f;
				ndir-=360.0f;
			}

			tilt-=my()*spd1;
			if(tilt<0.001f) tilt=0.001f;
			else if(tilt>100.0f) tilt=100.0f;
		}

		if(mw()!=0.0f)
		{
			zoom-=mw();
			if(zoom<7.0f) zoom=7.0f;
			else if(zoom>12.0f) zoom=12.0f;
		}
	}
	if(mp(3)) zoom=10.0f;

	ndir-=(ndir-dir)*spd2;
	r=d2r(ndir);
	dc=cosf(r);
	ds=sinf(r);

	ntilt-=(ntilt-tilt)*spd2;
	r=d2r(ntilt);
	tc=cosf(r);
	ts=sinf(r);

	nzoom-=(nzoom-zoom)*spd2;

	look(view,v3(ds*ts*nzoom,dc*ts*nzoom,3.0f+(tc*nzoom)),v3(0.0f,0.0f,3.0f));
	mult(pview,proj,view);

	send_pview(pview);

	/* ----- */
	use_fb(fbos[0]);
	clear();
	/* ----- */
	use_tex(area_tex);
	/* ----- */
	draw_vbo(&area_mod);
	use_fb(0);

	blit_fb(fbos[0],fbos[1],nsw,nsh);
	/* ----- */

	use_fb(fbos[2]);
	set_drawbufs(2);
	clear();
	draw_vbo(&area_mod);
	use_fb(0);

	use_mblur();
	use_tex(texs[0]); /* ----- */
	active_tex(GL_TEXTURE1,texs[2]);
	default_tex();

	use_fb(fbos[3]);
	set_drawbufs(1);
	clear();
	quad();
	use_fb(0);

	active_tex(GL_TEXTURE1,0);
	default_tex();

	/*viewport(0,0,sw,sh);*/
	use_vig();
	use_tex(texs[3]);

	clear();
	quad();

	if(kp(SDL_SCANCODE_DELETE) || kp(SDL_SCANCODE_ESCAPE))
		quit();
}
/*inline*/ vertex3f vertex3f::operator*(const float op) const {
    return mult(op);
}
Exemple #14
0
int main(int argc, char**argv)
{
				int block_sz = atoi(argv[1]);
				int blocks_nr = atoi(argv[2]);
				int edge_sz = atoi(argv[3]);

				std::vector<Block> bs(blocks_nr, Block(block_sz,edge_sz) );
				Edge e(edge_sz,blocks_nr,block_sz); 

				for (int i=0; i < bs.size(); ++i)
								bs[i].fill();
				e.fill();

				//    for (int i=0; i < bs.size(); ++i)
				//		std::cout << bs[i] << '\n';
				//	std::cout << e << '\n';

				std::vector<Block> bs0 = bs;
				Edge e0 = e; 

				double total_time=0.0;
				clock_t st, end;
				st = clock();
				for (int i=0; i < bs.size(); ++i)
								bs[i].fwd();
				end = clock();

				total_time += double((end - st)) / CLOCKS_PER_SEC;

				for (int b=0; b < bs.size(); ++b)
				{
								for (int br=0; br < bs[b].rows; ++br)
								{
												double& diag = bs[b](br,br);
												RowNO r;
												r.v = &diag;
												r.sz = bs[b].cols-br;
												r.row_no = bs[b].rows*b + br;

												st = clock();
												elim_col(e,r);
												end = clock();
												total_time += double((end - st)) / CLOCKS_PER_SEC;
								}
				}
				st = clock();
				e.fwd();
				e.bwd();

				for (int i=0; i < bs.size(); ++i){
								bs[i].bwd(e.edge_soln);
				}

				end = clock();
				total_time += double((end - st)) / CLOCKS_PER_SEC;
				std::cout << "elapsed " << total_time << '\n';

				std::vector<double> soln(bs[0].rows*bs.size() + edge_sz);

				for (int i=0; i < bs.size(); ++i)
								for (int k=0; k < bs[i].sol.size(); ++k)
												soln[i*block_sz+k] = bs[i].sol[k];

				for (int k=0; k < e.edge_soln.size(); ++k)
								soln[blocks_nr*block_sz+k] = e.edge_soln[k];

				//	for (int k=0; k < soln.size(); ++k)
				//	    std::cout << soln[k] << '\n';

				std::vector<double> Rhs;

				for(int i = 0; i < bs0.size(); ++i)
				{
								for(int r=0; r < bs0[i].rows; ++r)
												Rhs.push_back(bs0[i](r,bs0[i].cols-1));
				}

				for(int i = 0; i < e0.rows; ++i)
				{
								Rhs.push_back(e0(i,e0.cols-1));
				}

				std::vector<double> rhs = mult(bs0,e0,soln);


				double residue=0.0;
				for (int i=0; i < rhs.size(); ++i)
								residue += fabs(rhs[i] - Rhs[i]);
				std::cout << "\n\nres = " << residue << '\n';
}
Exemple #15
0
Node Node::operator*(const Node &node) const {
	return mult(node);	
}
Exemple #16
0
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
		inline void mult_right(const quaternion& right) {
			mult(*this, right);
		}
Exemple #17
0
void D_plsf_3(
    D_plsfState *st,   /* i/o: State struct                               */
    enum Mode mode,    /* i  : coder mode                                 */
    Word16 bfi,        /* i  : bad frame indicator (set to 1 if a         */
                       /*      bad frame is received)                     */
    Word16 * indice,   /* i  : quantization indices of 3 submatrices, Q0  */
    Word16 * lsp1_q    /* o  : quantized 1st LSP vector,              Q15 */
)
{
    Word16 i, index;
    Word16 *p_cb1, *p_cb2, *p_cb3, *p_dico, temp;
    Word16 lsf1_r[M];
    Word16 lsf1_q[M];
    

    if (bfi != 0)   /* if bad frame */
    {
        /* use the past LSFs slightly shifted towards their mean */

        for (i = 0; i < M; i++)
        {
            /* lsfi_q[i] = ALPHA*past_lsf_q[i] + ONE_ALPHA*mean_lsf[i]; */

            lsf1_q[i] = add(mult(st->past_lsf_q[i], ALPHA),
                            mult(mean_lsf[i], ONE_ALPHA));

        }

        /* estimate past quantized residual to be used in next frame */

	if (mode != MRDTX) {
	  for (i = 0; i < M; i++) {
            /* temp  = mean_lsf[i] +  past_r2_q[i] * PRED_FAC; */
	    
            temp = add(mean_lsf[i], mult(st->past_r_q[i], pred_fac[i]));
	    
            st->past_r_q[i] = sub(lsf1_q[i], temp);
	  }
	} else {
	  for (i = 0; i < M; i++) {
            /* temp  = mean_lsf[i] +  past_r2_q[i]; */
	    
            temp = add(mean_lsf[i], st->past_r_q[i]);
            st->past_r_q[i] = sub(lsf1_q[i], temp);
	  }	  
	}
    }
    else  /* if good LSFs received */
    {

       if (mode == MR475 || mode == MR515)
       {   /* MR475, MR515 */
          p_cb1 = dico1_lsf;
          p_cb2 = dico2_lsf;
          p_cb3 = mr515_3_lsf;
       }
       else if (mode == MR795)
       {   /* MR795 */

          p_cb1 = mr795_1_lsf;
          p_cb2 = dico2_lsf;
          p_cb3 = dico3_lsf;
       }
       else 
       {   /* MR59, MR67, MR74, MR102, MRDTX */

          p_cb1 = dico1_lsf;
          p_cb2 = dico2_lsf;
          p_cb3 = dico3_lsf;
       }
       
       /* decode prediction residuals from 3 received indices */

        index = *indice++;
        p_dico = &p_cb1[/*add(index, add(index, index))*/ 3 * index];
        lsf1_r[0] = *p_dico++;
        lsf1_r[1] = *p_dico++;
        lsf1_r[2] = *p_dico++;

        index = *indice++;
        

        if (mode == MR475 || mode == MR515)
        {   /* MR475, MR515 only using every second entry */
            /*index = shl(index,1);*/
          index <<= 1;
        }
        
        p_dico = &p_cb2[add(index, add(index, index))];
        lsf1_r[3] = *p_dico++;
        lsf1_r[4] = *p_dico++;
        lsf1_r[5] = *p_dico++;

        index = *indice++;
        p_dico = &p_cb3[shl(index, 2)];
        lsf1_r[6] = *p_dico++;
        lsf1_r[7] = *p_dico++;
        lsf1_r[8] = *p_dico++;
        lsf1_r[9] = *p_dico++;

        /* Compute quantized LSFs and update the past quantized residual */

        if (mode != MRDTX)
           for (i = 0; i < M; i++) {
              temp = add(mean_lsf[i], mult(st->past_r_q[i], pred_fac[i]));
              lsf1_q[i] = add(lsf1_r[i], temp);
              st->past_r_q[i] = lsf1_r[i];
           }
        else
           for (i = 0; i < M; i++) {
              temp = add(mean_lsf[i], st->past_r_q[i]);
              lsf1_q[i] = add(lsf1_r[i], temp);
              st->past_r_q[i] = lsf1_r[i];
           }
    }

    /* verification that LSFs has minimum distance of LSF_GAP Hz */

    Reorder_lsf(lsf1_q, LSF_GAP, M);

    Copy (lsf1_q, st->past_lsf_q, M);

    /*  convert LSFs to the cosine domain */

    Lsf_lsp(lsf1_q, lsp1_q, M);
}
Exemple #18
0
void pulsesequence() {
  /* Internal variable declarations *************************/
  int     shapelist90,shapelist180;
  double  seqtime,tau1,tau2,tau3,te1_delay,te2_delay,te3_delay,tr_delay;
  double  freq90[MAXNSLICE], freq180[MAXNSLICE];
  
  /* Diffusion variables */
  double  te1, te1min, del1, del2, del3, del4;
  double  te_diff1, te_diff2, tmp1, tmp2;
  double  diffamp;
  char    diffpat[MAXSTR];
  
  /* Navigator variables */
  double  etlnav;
  
  /* Variable crushers */
  double  cscale;
  double  vcrush;  // flag

  /* Diffusion parameters */
#define MAXDIR 1024           /* Will anybody do more than 1024 directions or b-values? */
  double roarr[MAXDIR], pearr[MAXDIR], slarr[MAXDIR];
  int    nbval,               /* Total number of bvalues*directions */
         nbro, nbpe, nbsl,
	 i;    
  double bro[MAXDIR], bpe[MAXDIR], bsl[MAXDIR], /* b-values along RO, PE, SL */
         brs[MAXDIR], brp[MAXDIR], bsp[MAXDIR], /* and the cross-terms */
	 btrace[MAXDIR],                        /* and the trace */
	 max_bval=0,
         dcrush, dgss2,       /* "delta" for crusher and gss2 gradients */
         Dro, Dcrush, Dgss2;  /* "DELTA" for readout, crusher and gss2 gradients */

  /* Real-time variables used in this sequence **************/
  int  vpe_ctr     = v1;      // PE loop counter
  int  vpe_mult    = v2;      // PE multiplier, ranges from -PE/2 to PE/2
  int  vpe2_ctr    = v3;      // PE loop counter
  int  vpe2_mult   = v4;      // PE multiplier, ranges from -PE/2 to PE/2
  int  vpe2_offset = v5;
  int  vpe2_steps  = v6;
  int  vms_slices  = v7;      // Number of slices
  int  vms_ctr     = v8;      // Slice loop counter
  int  vseg        = v9;      // Number of ETL segments 
  int  vseg_ctr    = v10;      // Segment counter
  int  vetl        = v11;      // Echo train length
  int  vetl_ctr    = v12;      // Echo train loop counter
  int  vssc        = v13;     // Compressed steady-states
  int  vtrimage    = v14;     // Counts down from nt, trimage delay when 0
  int  vacquire    = v15;     // Argument for setacqvar, to skip steady state acquires
  int  vphase180   = v16;     // phase of 180 degree refocusing pulse
  int  vetl_loop   = v17;     // Echo train length MINUS ONE, used on etl loop
  int  vnav        = v18;     // Echo train length
  int  vcr_ctr     = v19;     // variable crusher, index into table
  int  vcr1        = v20;     // multiplier along RO
  int  vcr2        = v21;     // multiplier along PE
  int  vcr3        = v22;     // multiplier along SL
  int  vetl1       = v23;     // = etl-1, determine navigator echo location in echo loop
  int  vcr_reset   = v24;     // check for navigator echoes, reset crushers

  /* Initialize paramaters **********************************/
  get_parameters();
  te1    = getval("te1");     /* te1 is the echo time for the first echo */
  cscale = getval("cscale");  /* Scaling factor on first 180 crushers */
  vcrush = getval("vcrush");  /* Variable crusher or set amplitude? */
  getstr("diffpat",diffpat);


  /*  Load external PE table ********************************/
  if (strcmp(petable,"n") && strcmp(petable,"N") && strcmp(petable,"")) {
    loadtable(petable);
  } else {
    abort_message("petable undefined");
  }

  /* Hold variable crushers in tables 5, 6, 7 */
  settable(t5,8,crro);
  settable(t6,8,crpe);    
  settable(t7,8,crss);
    
  seqtime = 0.0;
  espmin = 0.0;

  /* RF Power & Bandwidth Calculations **********************/
  init_rf(&p1_rf,p1pat,p1,flip1,rof1,rof2);
  init_rf(&p2_rf,p2pat,p2,flip2,rof1,rof2);
//  shape_rf(&p1_rf,"p1",p1pat,p1,flip1,rof1,rof2);
//  shape_rf(&p2_rf,"p2",p2pat,p2,flip2,rof1,rof2);
  calc_rf(&p1_rf,"tpwr1","tpwr1f");
  calc_rf(&p2_rf,"tpwr2","tpwr2f");
 
  /* Initialize gradient structures *************************/
  init_readout(&ro_grad,"ro",lro,np,sw); 
  init_readout_refocus(&ror_grad,"ror");
  init_phase(&pe_grad,"pe",lpe,nv);
  init_phase(&pe2_grad,"pe2",lpe2,nv2);
  init_slice(&ss_grad,"ss",thk);   /* NOTE assume same band widths for p1 and p2 */     
  init_slice(&ss2_grad,"ss2",thk);   /* not butterfly, want to scale crushers w/ echo */
  init_slice_refocus(&ssr_grad,"ssr");

  /* Gradient calculations **********************************/
  calc_readout(&ro_grad,WRITE,"gro","sw","at");
  calc_readout_refocus(&ror_grad,&ro_grad,NOWRITE,"gror");
  calc_phase(&pe_grad,NOWRITE,"gpe","tpe");
  calc_phase(&pe2_grad,NOWRITE,"gpe2","tpe2");
  calc_slice(&ss_grad,&p1_rf,WRITE,"gss");
  calc_slice(&ss2_grad,&p1_rf,WRITE,"");
  calc_slice_refocus(&ssr_grad,&ss_grad,WRITE,"gssr");

  /* Equalize refocus and PE gradient durations *************/
  calc_sim_gradient(&ror_grad,&pe_grad,&pe2_grad,0.0,WRITE);

  /* Variable crusher */
  init_generic(&crush_grad,"crush",gcrush,tcrush);
  calc_generic(&crush_grad,WRITE,"","");

  /* Create optional prepulse events ************************/
  if (sat[0]  == 'y') create_satbands();
  if (fsat[0] == 'y') create_fatsat();
  if (mt[0]   == 'y') create_mtc();
  
  /* Optional Diffusion gradient */
  if (diff[0] == 'y') {
    init_generic(&diff_grad,"diff",gdiff,tdelta);
    if (!strcmp("sine",diffpat)) {
      diff_grad.shape = SINE;
      diffamp         = gdiff*1;
    }
 
    /* adjust duration, so tdelta is from start ramp up to start ramp down */   
    if ((ix == 1) && (diff_grad.shape == TRAPEZOID)) {
      calc_generic(&diff_grad,NOWRITE,"","");
      diff_grad.duration += diff_grad.tramp; 
    }
    calc_generic(&diff_grad,WRITE,"","");  
  }

  /* Set up frequency offset pulse shape list ********/
  offsetlist(pss,ss_grad.amp,0,freq90,ns,seqcon[1]);
  offsetlist(pss,ss2_grad.ssamp,0,freq180,ns,seqcon[1]);
  shapelist90  = shapelist(p1_rf.pulseName,ss_grad.rfDuration, freq90, ns,0,seqcon[1]);
  shapelist180 = shapelist(p2_rf.pulseName,ss2_grad.rfDuration,freq180,ns,0,seqcon[1]);

  /* same slice selection gradient and RF pattern used */
  if (ss_grad.rfFraction != 0.5)
    abort_message("RF pulse must be symmetric (RF fraction = %.2f)",ss_grad.rfFraction);
  if (ro_grad.echoFraction != 1)
    abort_message("Echo Fraction must be 1");


  /*****************************************************/
  /* TIMING FOR ECHOES *********************************/
  /*****************************************************/
  /* First echo time, without diffusion */
  tau1 = ss_grad.rfCenterBack + ssr_grad.duration + crush_grad.duration + ss2_grad.rfCenterFront;
  tau2 = ss2_grad.rfCenterBack + crush_grad.duration + pe_grad.duration + ro_grad.timeToEcho;
  te1min = 2*MAX(tau1,tau2);
  if (te1 < te1min + 2*4e-6) {
    abort_message("First echo time too small, minimum is %.2fms\n",(te1min+2*4e-6)*1000);
  }

  /* Each half-echo period in the ETL loop ********/
  tau3 = ro_grad.timeFromEcho + pe_grad.duration + crush_grad.duration + ss2_grad.rfCenterFront;
  espmin = 2*MAX(tau2,tau3);   // Minimum echo spacing
  if (minesp[0] == 'y') {
    esp = espmin + 2*4e-6;
    putvalue("esp",esp);
  }
  if (esp - (espmin + 2*4e-6) < -12.5e-9) {
    abort_message("Echo spacing too small, minimum is %.2fms\n",(espmin+2*4e-6)*1000);
  }


  te1_delay = te1/2.0 - tau1;
  te2_delay = esp/2.0 - tau2;
  te3_delay = esp/2.0 - tau3;


  /*****************************************************/
  /* TIMING FOR DIFFUSION ******************************/
  /*****************************************************/
  del1 = te1/2.0 - tau1;
  del2 = 0;
  del3 = te1/2.0 - tau2;
  del4 = 0;

  if (diff[0] == 'y') {
    tau1 += diff_grad.duration;
    tau2 += diff_grad.duration;

    te1min = 2*MAX(tau1,tau2);
    if (te1 < te1min + 4*4e-6) {  /* te1 is split into 4 delays, each of which must be >= 4us */
      abort_message("ERROR %s: First echo time too small, minimum is %.2fms\n",seqfil,te1min*1000);
    }

    /* te1 is the echo time for the first echo */
    te_diff1 = te1/2 - tau1;  /* Available time in first half of first echo */
    te_diff2 = te1/2 - tau2;  /* Available time in second half of first echo */

    tmp1 = ss2_grad.duration + 2*crush_grad.duration;  /* duration of 180 block */
    /* Is tDELTA long enough? */
    if (tDELTA < diff_grad.duration + tmp1)
      abort_message("DELTA too short, increase to %.2fms",
        (diff_grad.duration + tmp1)*1000);

    /* Is tDELTA too long? */
    tmp2 = diff_grad.duration + te_diff1 + tmp1 + te_diff2;
    if (tDELTA > tmp2) {
      abort_message("DELTA too long, increase te1 to %.2fms",
        (te1 + (tDELTA-tmp2))*1000);
    }

    /* First attempt to put lobes right after slice select, ie del1 = 0 */
    del1 = 4e-6;  /* At least 4us after setting power for 180 */
    del2 = te_diff1 - del1;
    del3 = tDELTA - (diff_grad.duration + del2 + tmp1);
    
    if (del3 < 4e-6) {  /* shift diffusion block towards acquisition */
      del3 = 4e-6;
      del2 = tDELTA - (diff_grad.duration + tmp1 + del3);
    }

    del1 = te_diff1 - del2;
    del4 = te_diff2 - del3;
    
    if (fabs(del4) < 12.5e-9) del4 = 0;
  
  }
  te = te1 + (kzero-1)*esp;                // Return effective TE
  putvalue("te",te);

  /* How many echoes in the echo loop, including navigators? */
  etlnav = (etl-1)+(navigator[0]=='y')*2.0;
  
  /* Minimum TR **************************************/
  seqtime  = 4e-6 + 2*nseg*ns*4e-6;  /* count all the 4us delays */
  seqtime += ns*(ss_grad.duration/2 + te1 + (etlnav)*esp + ro_grad.timeFromEcho + pe_grad.duration + te3_delay);

  /* Increase TR if any options are selected****************/
  if (sat[0] == 'y')  seqtime += ns*satTime;
  if (fsat[0] == 'y') seqtime += ns*fsatTime;
  if (mt[0] == 'y')   seqtime += ns*mtTime;

  trmin = seqtime + ns*4e-6;  /* Add 4us to ensure that tr_delay is always >= 4us */
  if (mintr[0] == 'y'){
    tr = trmin;
    putvalue("tr",tr+1e-6);
  }
  if (tr < trmin) {
    abort_message("TR too short.  Minimum TR = %.2fms\n",trmin*1000);
  }
  tr_delay = (tr - seqtime)/ns;


  /* Set number of segments for profile or full image **********/
  nseg      = prep_profile(profile[0],nv/etl,&pe_grad,&per_grad);
  pe2_steps = prep_profile(profile[1],nv2,&pe2_grad,&pe2r_grad);

  /* Calculate total scan time */
  g_setExpTime(tr*(nt*nseg*pe2_steps*arraydim + ssc));




  /***************************************************/
  /* CALCULATE B VALUES ******************************/
  if (diff[0] == 'y') {
    /* Get multiplication factors and make sure they have same # elements */
    /* All this is only necessary because putCmd only work for ix==1      */
    nbro = (int) getarray("dro",roarr);  nbval = nbro;
    nbpe = (int) getarray("dpe",pearr);  if (nbpe > nbval) nbval = nbpe;
    nbsl = (int) getarray("dsl",slarr);  if (nbsl > nbval) nbval = nbsl;
    if ((nbro != nbval) && (nbro != 1))
      abort_message("%s: Number of directions/b-values must be the same for all axes (readout)",seqfil);
    if ((nbpe != nbval) && (nbpe != 1))
      abort_message("%s: Number of directions/b-values must be the same for all axes (phase)",seqfil);
    if ((nbsl != nbval) && (nbsl != 1))
      abort_message("%s: Number of directions/b-values must be the same for all axes (slice)",seqfil);


    if (nbro == 1) for (i = 1; i < nbval; i++) roarr[i] = roarr[0];
    if (nbpe == 1) for (i = 1; i < nbval; i++) pearr[i] = pearr[0];
    if (nbsl == 1) for (i = 1; i < nbval; i++) slarr[i] = slarr[0];

  }
  else {
    nbval = 1;
    roarr[0] = 0;
    pearr[0] = 0;
    slarr[0] = 0;
  }

  for (i = 0; i < nbval; i++)  {
    dcrush = crush_grad.duration;       //"delta" for crusher
    Dcrush = dcrush + ss_grad.duration; //"DELTA" for crusher

    /* Readout */
    Dro     = ror_grad.duration;
    bro[i]  = bval(gdiff*roarr[i],tdelta,tDELTA);
    bro[i] += bval(ro_grad.amp,ro_grad.timeToEcho,Dro);
    bro[i] += bval(crush_grad.amp,dcrush,Dcrush);
    bro[i] += bval_nested(gdiff*roarr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);

    /* Slice */
    dgss2   = Dgss2 = ss_grad.rfCenterFront;
    bsl[i]  = bval(gdiff*slarr[i],tdelta,tDELTA);
    bsl[i] += bval(crush_grad.amp,dcrush,Dcrush);
    bsl[i] += bval(ss2_grad.ssamp,dgss2,Dgss2);
    bsl[i] += bval_nested(gdiff*slarr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);
    bsl[i] += bval_nested(gdiff*slarr[i],tdelta,tDELTA,ss2_grad.ssamp,dgss2,Dgss2);
    bsl[i] += bval_nested(ss2_grad.ssamp,dgss2,Dgss2,
                crush_grad.amp,dcrush,Dcrush);

    /* Phase */
    bpe[i]  = bval(gdiff*pearr[i],tdelta,tDELTA);
    bpe[i] += bval(crush_grad.amp,dcrush,Dcrush);
    bpe[i] += bval_nested(gdiff*pearr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);

    /* Readout/Slice Cross-terms */
    brs[i]  = bval2(gdiff*roarr[i],gdiff*slarr[i],tdelta,tDELTA);
    brs[i] += bval2(crush_grad.amp,
                    crush_grad.amp,dcrush,Dcrush);
    brs[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);
    brs[i] += bval_cross(gdiff*slarr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);
    brs[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA,
                ss2_grad.ssamp,dgss2,Dgss2);
    brs[i] += bval_cross(crush_grad.amp,dcrush,Dcrush,
                ss2_grad.ssamp,dgss2,Dgss2);

    /* Readout/Phase Cross-terms */
    brp[i]  = bval2(gdiff*roarr[i],gdiff*pearr[i],tdelta,tDELTA);
    brp[i] += bval2(crush_grad.amp,
                    crush_grad.amp,dcrush,Dcrush);
    brp[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);
    brp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);

    /* Slice/Phase Cross-terms */
    bsp[i]  = bval2(gdiff*pearr[i],gdiff*slarr[i],tdelta,tDELTA);
    bsp[i] += bval2(crush_grad.amp,
                    crush_grad.amp,dcrush,Dcrush);
    bsp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);
    bsp[i] += bval_cross(gdiff*slarr[i],tdelta,tDELTA,
                crush_grad.amp,dcrush,Dcrush);
    bsp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA,
                ss2_grad.ssamp,dgss2,Dgss2);
    bsp[i] += bval_cross(crush_grad.amp,dcrush,Dcrush,
                ss2_grad.ssamp,dgss2,Dgss2);


    btrace[i] = (bro[i]+bsl[i]+bpe[i]);

    if (max_bval < btrace[i]) {
      max_bval = (bro[i]+bsl[i]+bpe[i]);
    }
  }  /* End for-all-directions */

  putarray("bvalrr",bro,nbval);
  putarray("bvalpp",bpe,nbval);
  putarray("bvalss",bsl,nbval);
  putarray("bvalrp",brp,nbval);
  putarray("bvalrs",brs,nbval);
  putarray("bvalsp",bsp,nbval);
  putarray("bvalue",btrace,nbval);
  putvalue("max_bval",max_bval);





  /* Shift DDR for pro *******************************/
  roff = -poffset(pro,ro_grad.roamp);


  /* PULSE SEQUENCE *************************************/
  if (ix == 1) grad_advance(tep);
  initval(fabs(ssc),vssc);      // Compressed steady-state counter
  setacqvar(vacquire);          // Control acquisition through vacquire
  assign(one,vacquire);         // Turn on acquire when vacquire is zero

  /* Phase cycle: Alternate 180 phase to cancel residual FID */
  mod2(ct,vphase180);           // 0101
  dbl(vphase180,vphase180);     // 0202
  add(vphase180,one,vphase180); // 1313 Phase difference from 90
  add(vphase180,oph,vphase180);

  obsoffset(resto);
  delay(4e-6);
    
  initval(nseg,vseg);
  initval(pe2_steps/2.0,vpe2_offset);
  
  initval(etl,vetl);
  initval(etl-1,vetl1);

  peloop2(seqcon[3],pe2_steps,vpe2_steps,vpe2_ctr);
  /* Use standard encoding order for 2nd PE dimension */
  sub(vpe2_ctr,vpe2_offset,vpe2_mult);

    loop(vseg,vseg_ctr);

      /* Compressed steady-states: 1st array & transient, all arrays if ssc is negative */
      if ((ix > 1) && (ssc > 0))
	assign(zero,vssc);
      sub(vseg_ctr,vssc,vseg_ctr);   // vpe_ctr counts up from -ssc
      assign(zero,vssc);
      ifzero(vseg_ctr);
	assign(zero,vacquire);       // Start acquiring when vseg_ctr reaches zero
      endif(vseg_ctr);

      msloop(seqcon[1],ns,vms_slices,vms_ctr);
	if (ticks) {
          xgate(ticks);
          grad_advance(tep);
	}
	sp1on(); delay(4e-6); sp1off();    // Scope trigger

	/* Prepulse options ***********************************/
	if (sat[0]  == 'y') satbands();
	if (fsat[0] == 'y') fatsat();
	if (mt[0]   == 'y') mtc();

	/* 90 degree pulse ************************************/         
	rotate();
	obspower(p1_rf.powerCoarse);
	obspwrf(p1_rf.powerFine);
	delay(4e-6);
	obl_shapedgradient(ss_grad.name,ss_grad.duration,0,0,ss_grad.amp,NOWAIT);   
	delay(ss_grad.rfDelayFront);
	shapedpulselist(shapelist90,ss_grad.rfDuration,oph,rof1,rof2,seqcon[1],vms_ctr);
	delay(ss_grad.rfDelayBack);

	/* Read dephase and Slice refocus *********************/
	obl_shapedgradient(ssr_grad.name,ssr_grad.duration,0.0,0.0,-ssr_grad.amp,WAIT);

	/* First half-TE delay ********************************/
	obspower(p2_rf.powerCoarse);
	obspwrf(p2_rf.powerFine);
	delay(del1);

	/* DIFFUSION GRADIENT */
	if (diff[0] == 'y')
	  obl_shapedgradient(diff_grad.name,diff_grad.duration,diff_grad.amp*dro,diff_grad.amp*dpe,diff_grad.amp*dsl,WAIT);

	delay(del2);


	/*****************************************************/
	    /* FIRST ECHO OUTSIDE LOOP ***************************/
	/*****************************************************/
	ifzero(vacquire);  // real acquisition, get PE multiplier from table
          mult(vseg_ctr,vetl,vpe_ctr);
          getelem(t1,vpe_ctr,vpe_mult);
	elsenz(vacquire);  // steady state scan 
          assign(zero,vpe_mult);
	endif(vacquire);

	/* Variable crusher */
	assign(zero,vcr_ctr);
	getelem(t5,vcr_ctr,vcr1); 
	getelem(t6,vcr_ctr,vcr2);	     
	getelem(t7,vcr_ctr,vcr3);

  if(vcrush) 
	phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
	  crush_grad.duration,
	  (double)0,(double)0,(double)0,                                     // base levels
	  crush_grad.amp*cscale,crush_grad.amp*cscale,crush_grad.amp*cscale, // step size
	  vcr1,vcr2,vcr3,                                                    // multipliers
	  (double)1.0,(double)1.0,(double)1.0,                               // upper limit on multipliers
	  1,WAIT,0);

  else 
  obl_shapedgradient(crush_grad.name,crush_grad.duration,
    crush_grad.amp,crush_grad.amp,crush_grad.amp,WAIT);

	/* 180 degree pulse *******************************/
	obl_shapedgradient(ss2_grad.name,ss2_grad.duration,0,0,ss2_grad.amp,NOWAIT);   
	delay(ss2_grad.rfDelayFront); 
	shapedpulselist(shapelist180,ss2_grad.rfDuration,vphase180,rof1,rof2,seqcon[1],vms_ctr);
	delay(ss2_grad.rfDelayBack);   

  if (vcrush)
	phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
	  crush_grad.duration,
	  (double)0,(double)0,(double)0,                                     // base levels
	  crush_grad.amp*cscale,crush_grad.amp*cscale,crush_grad.amp*cscale, // step size
	  vcr1,vcr2,vcr3,                                                    // multipliers
	  (double)1.0,(double)1.0,(double)1.0,                               // upper limit on multipliers
	  1,WAIT,0);
  else
  obl_shapedgradient(crush_grad.name,crush_grad.duration,
    crush_grad.amp,crush_grad.amp,crush_grad.amp,WAIT);

	delay(del3);

	/* DIFFUSION GRADIENT */
	if (diff[0] == 'y')
	  obl_shapedgradient(diff_grad.name,diff_grad.duration,diff_grad.amp*dro,diff_grad.amp*dpe,diff_grad.amp*dsl,WAIT);

	delay(del4);

	/* Phase-encode gradient ******************************/
	pe2_shapedgradient(pe_grad.name,pe_grad.duration,-ror_grad.amp,0,0,
	  -pe_grad.increment,-pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);

	/* Readout gradient ************************************/
	obl_shapedgradient(ro_grad.name,ro_grad.duration,ro_grad.roamp,0,0,NOWAIT);
	delay(ro_grad.atDelayFront);

	/* Acquire data ****************************************/
	startacq(10e-6);
	acquire(np,1.0/sw);
	endacq();

	delay(ro_grad.atDelayBack);

	/* Rewinding phase-encode gradient ********************/
	pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0,
	  pe_grad.increment,pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);

	/* Second half-TE delay *******************************/
	delay(te3_delay);


	/*****************************************************/
	    /* LOOP THROUGH THE REST OF ETL **********************/
	/*****************************************************/
	peloop(seqcon[2],etlnav,vetl_loop,vetl_ctr);
	  ifzero(vacquire);  // real acquisition, get PE multiplier from table
            mult(vseg_ctr,vetl,vpe_ctr);
            add(vpe_ctr,vetl_ctr,vpe_ctr);
	    add(vpe_ctr,one,vpe_ctr);
            getelem(t1,vpe_ctr,vpe_mult);
    	  elsenz(vacquire);  // steady state scan 
	    assign(zero,vpe_mult);
	  endif(vacquire);

	  /* But don't phase encode navigator echoes */
          ifrtGE(vetl_ctr,vetl1,vnav);
	    assign(zero,vpe_mult);
	  endif(vnav);


    	  /* Variable crusher */
	  incr(vcr_ctr);  /* Get next crusher level */
	  /* Except if we're doing navigators, start over */
	  sub(vetl1,vetl_ctr,vcr_reset);
	  ifzero(vcr_reset);
	    assign(zero,vcr_ctr);
	  endif(vcr_reset);

    	  getelem(t5,vcr_ctr,vcr1); 
    	  getelem(t6,vcr_ctr,vcr2);	     
    	  getelem(t7,vcr_ctr,vcr3);

	  phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
	    crush_grad.duration,
	    (double)0,(double)0,(double)0,                                     // base levels
	    crush_grad.amp,crush_grad.amp,crush_grad.amp,                      // step size
	    vcr1,vcr2,vcr3,                                                    // multipliers
	    (double)1.0,(double)1.0,(double)1.0,                               // upper limit on multipliers
	    1,WAIT,0);

          /* 180 degree pulse *******************************/
          obl_shapedgradient(ss2_grad.name,ss2_grad.duration,0,0,ss2_grad.amp,NOWAIT);   
    	  delay(ss2_grad.rfDelayFront); 
          shapedpulselist(shapelist180,ss2_grad.rfDuration,vphase180,rof1,rof2,seqcon[1],vms_ctr);
          delay(ss2_grad.rfDelayBack);   

          /* Variable crusher */
	  phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
	    crush_grad.duration,
	    (double)0,(double)0,(double)0,                                     // base levels
	    crush_grad.amp,crush_grad.amp,crush_grad.amp,                      // step size
	    vcr1,vcr2,vcr3,                                                    // multipliers
	    (double)1.0,(double)1.0,(double)1.0,                               // upper limit on multipliers
	    1,WAIT,0);

          /* Phase-encode gradient ******************************/
	  pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0,
	    -pe_grad.increment,-pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);

          /* Second half-TE period ******************************/
	  delay(te2_delay);

          /* Readout gradient ************************************/
          obl_shapedgradient(ro_grad.name,ro_grad.duration,ro_grad.roamp,0,0,NOWAIT);
          delay(ro_grad.atDelayFront);
          startacq(10e-6);
          acquire(np,1.0/sw);
	  endacq();
          delay(ro_grad.atDelayBack);

          /* Rewinding phase-encode gradient ********************/
	  pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0,
	    pe_grad.increment,pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);

          /* Second half-TE delay *******************************/
          delay(te3_delay);
	endpeloop(seqcon[2],vetl_ctr);

	/* Relaxation delay ***********************************/
	if (!trtype)
          delay(tr_delay);
      endmsloop(seqcon[1],vms_ctr);
      if (trtype)
	delay(ns*tr_delay);
    endloop(vseg_ctr);
  endpeloop(seqcon[3],vpe2_ctr);

  /* Inter-image delay **********************************/
  sub(ntrt,ct,vtrimage);
  decr(vtrimage);
  ifzero(vtrimage);
    delay(trimage);
  endif(vtrimage);
}
Exemple #19
0
    Word16 code_2i40_9bits(
        Word16 subNr,       /* i : subframe number                          */
        Word16 x[],         /* i : target vector                            */
        Word16 h[],         /* i : impulse response of weighted synthesis   */
        /*     filter h[-L_subfr..-1] must be set to 0. */
        Word16 T0,          /* i : Pitch lag                                */
        Word16 pitch_sharp, /* i : Last quantized pitch gain                */
        Word16 code[],      /* o : Innovative codebook                      */
        Word16 y[],         /* o : filtered fixed codebook excitation       */
        Word16 * sign,      /* o : Signs of 2 pulses                        */
        Flag   * pOverflow  /* o : Flag set when overflow occurs            */
    )
    {
        Word16 codvec[NB_PULSE];
        Word16 dn[L_CODE];
        Word16 dn2[L_CODE];
        Word16 dn_sign[L_CODE];
        Word16 rr[L_CODE][L_CODE];

        register Word16 i;

        Word16 index;
        Word16 sharp;
        Word16 temp;
        Word32 L_temp;

        L_temp = ((Word32) pitch_sharp) << 1;

        /* Check for overflow condition */
        if (L_temp != (Word32)((Word16) L_temp))
        {
            *(pOverflow) = 1;
            sharp = (pitch_sharp > 0) ? MAX_16 : MIN_16;
        }
        else
        {
            sharp = (Word16) L_temp;
        }

        if (T0 < L_CODE)
        {
            for (i = T0; i < L_CODE; i++)
            {
                temp =
                    mult(
                        *(h + i - T0),
                        sharp,
                        pOverflow);

                *(h + i) =
                    add(
                        *(h + i),
                        temp,
                        pOverflow);
            }
        }

        cor_h_x(
            h,
            x,
            dn,
            1,
            pOverflow);

        /* dn2[] not used in this codebook search */

        set_sign(
            dn,
            dn_sign,
            dn2,
            8);

        cor_h(
            h,
            dn_sign,
            rr,
            pOverflow);

        search_2i40(
            subNr,
            dn,
            rr,
            codvec,
            pOverflow);

        index =
            build_code(
                subNr,
                codvec,
                dn_sign,
                code,
                h,
                y,
                sign,
                pOverflow);

        /*-----------------------------------------------------------------*
         * Compute innovation vector gain.                                 *
         * Include fixed-gain pitch contribution into code[].              *
         *-----------------------------------------------------------------*/

        if (T0 < L_CODE)
        {
            for (i = T0; i < L_CODE; i++)
            {
                temp =
                    mult(
                        *(code + i - T0),
                        sharp,
                        pOverflow);

                *(code + i) =
                    add(
                        *(code + i),
                        temp,
                        pOverflow);
            }
        }

        return(index);
    }
Vec reflection_reflection(const Vec &x , int depth, const Sphere &sph, const Ray &r, const Vec &n, const Vec &f){
  Ray rr2; 
    rr2.origin = x; 
    rr2.direction = r.direction-n*2*dot(n,r.direction);
    return sph.light + mult(f,calculate_light(rr2,depth));
}
Exemple #21
0
/*************************************************************************
 *
 *  Function:   gain_adapt()
 *  Purpose:    calculate pitch/codebook gain adaptation factor alpha
 *              (and update the adaptor state)
 *
 **************************************************************************
 */
void gain_adapt(
    GainAdaptState *st,  /* i  : state struct                  */
    Word16 ltpg,         /* i  : ltp coding gain (log2()), Q13 */
    Word16 gain_cod,     /* i  : code gain,                Q1  */
    Word16 *alpha        /* o  : gain adaptation factor,   Q15 */
)
{
    Word16 adapt;      /* adaptdation status; 0, 1, or 2       */
    Word16 result;     /* alpha factor, Q13                    */
    Word16 filt;       /* median-filtered LTP coding gain, Q13 */
    Word16 tmp, i;
    
    /* basic adaptation */
    test ();
    if (sub (ltpg, LTP_GAIN_THR1) <= 0)
    {
        adapt = 0;                            move16 ();
    }
    else
    {
        test ();
        if (sub (ltpg, LTP_GAIN_THR2) <= 0)
        {
            adapt = 1;                        move16 ();
        }
        else
        {
            adapt = 2;                        move16 ();
        }
    }

    /*
     * // onset indicator
     * if ((cbGain > onFact * cbGainMem[0]) && (cbGain > 100.0))
     *     onset = 8;
     * else
     *     if (onset)
     *         onset--;
     */
    /* tmp = cbGain / onFact; onFact = 2.0; 200 Q1 = 100.0 */
    tmp = shr_r (gain_cod, 1);
    test (); test ();
    if ((sub (tmp, st->prev_gc) > 0) && sub(gain_cod, 200) > 0)
    {
        st->onset = 8;                            move16 ();
    }
    else
    {
        test ();
        if (st->onset != 0)
        {
            st->onset = sub (st->onset, 1);       move16 ();
        }
    }

    /*
     *  // if onset, increase adaptor state
     *  if (onset && (gainAdapt < 2)) gainAdapt++;
     */
    test(); test ();
    if ((st->onset != 0) && (sub (adapt, 2) < 0))
    {
        adapt = add (adapt, 1);
    }

    st->ltpg_mem[0] = ltpg;                       move16 ();
    filt = gmed_n (st->ltpg_mem, 5);   move16 (); /* function result */

    test ();
    if (adapt == 0)
    {
        test ();
        if (sub (filt, 5443) > 0) /* 5443 Q13 = 0.66443... */
        {
            result = 0; move16 ();
        }
        else
        {
            test ();
            if (filt < 0)
            {
                result = 16384; move16 ();  /* 16384 Q15 = 0.5 */
            }
            else
            {   /* result       =   0.5 - 0.75257499*filt     */
                /* result (Q15) = 16384 - 24660 * (filt << 2) */
                filt = shl (filt, 2); /* Q15 */
                result = sub (16384, mult (24660, filt));
            }
        }
    }
    else
    {
        result = 0; move16 ();
    }
    /*
     *  if (prevAlpha == 0.0) result = 0.5 * (result + prevAlpha);
     */
    test ();
    if (st->prev_alpha == 0)
    {
        result = shr (result, 1);
    }

    /* store the result */
    *alpha = result;                           move16 ();
    
    /* update adapter state memory */
    st->prev_alpha = result;                   move16 ();
    st->prev_gc = gain_cod;                    move16 ();

    for (i = LTPG_MEM_SIZE-1; i > 0; i--)
    {
        st->ltpg_mem[i] = st->ltpg_mem[i-1];   move16 ();
    }
    /* mem[0] is just present for convenience in calling the gmed_n[5]
     * function above. The memory depth is really LTPG_MEM_SIZE-1.
     */
}
Exemple #22
0
void cbsearch(Word16 x[],        /* i : target vector, Q0                     */
              Word16 h[],        /* i : impulse response of weighted synthesis*/
              /*     filter h[-L_subfr..-1] must be set to */
              /*     zero. Q12                             */
              Word16 T0,         /* i : Pitch lag                             */
              Word16 pitch_sharp,/* i : Last quantized pitch gain, Q14        */
              Word16 gain_pit,   /* i : Pitch gain, Q14                       */
              Word16 res2[],     /* i : Long term prediction residual, Q0     */
              Word16 code[],     /* o : Innovative codebook, Q13              */
              Word16 y[],        /* o : filtered fixed codebook excitation    */
              /*     Q12                                   */
              Word16 **anap,     /* o : Signs of the pulses                   */
              enum Mode mode,    /* i : coder mode                            */
              Word16 subNr,      /* i : subframe number                       */
              CommonAmrTbls* common_amr_tbls, /* ptr to struct of tables    */
              Flag  *pOverflow)  /* o : Flag set when overflow occurs         */
{
    Word16 index;
    Word16 i;
    Word16 temp;
    Word16 pit_sharpTmp;

    /* For MR74, the pre and post CB pitch sharpening is included in the
     * codebook search routine, while for MR122 is it not.
     */

    if ((mode == MR475) || (mode == MR515))
    {
        /* MR475, MR515 */
        *(*anap)++ =
            code_2i40_9bits(
                subNr,
                x,
                h,
                T0,
                pitch_sharp,
                code,
                y,
                &index,
                common_amr_tbls->startPos_ptr,
                pOverflow);

        *(*anap)++ = index;    /* sign index */
    }
    else if (mode == MR59)
    {   /* MR59 */
        *(*anap)++ =
            code_2i40_11bits(
                x,
                h,
                T0,
                pitch_sharp,
                code,
                y,
                &index,
                pOverflow);

        *(*anap)++ = index;    /* sign index */
    }
    else if (mode == MR67)
    {   /* MR67 */
        *(*anap)++ =
            code_3i40_14bits(
                x,
                h,
                T0,
                pitch_sharp,
                code,
                y,
                &index,
                pOverflow);

        *(*anap)++ = index;    /* sign index */
    }
    else if ((mode == MR74) || (mode == MR795))
    {   /* MR74, MR795 */
        *(*anap)++ =
            code_4i40_17bits(
                x,
                h,
                T0,
                pitch_sharp,
                code,
                y,
                &index,
                common_amr_tbls->gray_ptr,
                pOverflow);

        *(*anap)++ = index;    /* sign index */
    }
    else if (mode == MR102)
    {   /* MR102 */
        /*-------------------------------------------------------------*
         * - include pitch contribution into impulse resp. h1[]        *
         *-------------------------------------------------------------*/
        /* pit_sharpTmp = pit_sharp;                     */
        /* if (pit_sharpTmp > 1.0) pit_sharpTmp = 1.0;   */

        pit_sharpTmp =
            shl(
                pitch_sharp,
                1,
                pOverflow);

        for (i = T0; i < L_SUBFR; i++)
        {
            temp =
                mult(
                    h[i - T0],
                    pit_sharpTmp,
                    pOverflow);

            h[i] =
                add_16(
                    h[i],
                    temp,
                    pOverflow);
        }

        /*--------------------------------------------------------------*
         * - Innovative codebook search (find index and gain)           *
         *--------------------------------------------------------------*/
        code_8i40_31bits(
            x,
            res2,
            h,
            code,
            y,
            *anap,
            pOverflow);

        *anap += 7;

        /*-------------------------------------------------------*
         * - Add the pitch contribution to code[].               *
         *-------------------------------------------------------*/
        for (i = T0; i < L_SUBFR; i++)
        {
            temp =
                mult(
                    code[i - T0],
                    pit_sharpTmp,
                    pOverflow);

            code[i] =
                add_16(
                    code[i],
                    temp,
                    pOverflow);
        }
    }
    else
    {  /* MR122 */
        /*-------------------------------------------------------------*
         * - include pitch contribution into impulse resp. h1[]        *
         *-------------------------------------------------------------*/

        /* pit_sharpTmp = gain_pit;                      */
        /* if (pit_sharpTmp > 1.0) pit_sharpTmp = 1.0;   */

        pit_sharpTmp = shl(gain_pit, 1, pOverflow);

        for (i = T0; i < L_SUBFR; i++)
        {
            temp = ((Word32)h[i - T0] * pit_sharpTmp) >> 15;
            /*
                     mult(
                            h[i - T0],
                            ,
                            pOverflow);
            */
            h[i] =
                add_16(
                    h[i],
                    temp,
                    pOverflow);
        }
        /*--------------------------------------------------------------*
         * - Innovative codebook search (find index and gain)           *
         *--------------------------------------------------------------*/

        code_10i40_35bits(
            x,
            res2,
            h,
            code,
            y,
            *anap,
            common_amr_tbls->gray_ptr,
            pOverflow);

        *anap += 10;

        /*-------------------------------------------------------*
         * - Add the pitch contribution to code[].               *
         *-------------------------------------------------------*/
        for (i = T0; i < L_SUBFR; i++)
        {
            temp =
                mult(
                    code[i - T0],
                    pit_sharpTmp,
                    pOverflow);

            code[i] =
                add_16(
                    code[i],
                    temp,
                    pOverflow);
        }
    }

}
/*************************************************************************
 *
 * FUNCTION:  MR795_gain_code_quant3
 *
 * PURPOSE: Pre-quantization of codebook gains, given three possible
 *          LTP gains (using predicted codebook gain)
 *
 *************************************************************************/
void
MR795_gain_code_quant3(
    Word16 exp_gcode0,        /* i  : predicted CB gain (exponent), Q0  */
    Word16 gcode0,            /* i  : predicted CB gain (norm.),    Q14 */
    Word16 g_pitch_cand[],    /* i  : Pitch gain candidates (3),    Q14 */
    Word16 g_pitch_cind[],    /* i  : Pitch gain cand. indices (3), Q0  */
    Word16 frac_coeff[],      /* i  : coefficients (5),             Q15 */
    Word16 exp_coeff[],       /* i  : energy coefficients (5),      Q0  */
                              /*      coefficients from calc_filt_ener()*/
    Word16 *gain_pit,         /* o  : Pitch gain,                   Q14 */
    Word16 *gain_pit_ind,     /* o  : Pitch gain index,             Q0  */
    Word16 *gain_cod,         /* o  : Code gain,                    Q1  */
    Word16 *gain_cod_ind,     /* o  : Code gain index,              Q0  */
    Word16 *qua_ener_MR122,   /* o  : quantized energy error,       Q10 */
                              /*      (for MR122 MA predictor update)   */
    Word16 *qua_ener          /* o  : quantized energy error,       Q10 */
                              /*      (for other MA predictor update)   */
)
{
    const Word16 *p;
    Word16 i, j, cod_ind, pit_ind;
    Word16 e_max, exp_code;
    Word16 g_pitch, g2_pitch, g_code, g2_code_h, g2_code_l;
    Word16 g_pit_cod_h, g_pit_cod_l;
    Word16 coeff[5], coeff_lo[5];
    Word16 exp_max[5];
    Word32 L_tmp, L_tmp0, dist_min;

    /*
     * The error energy (sum) to be minimized consists of five terms, t[0..4].
     *
     *                      t[0] =    gp^2  * <y1 y1>
     *                      t[1] = -2*gp    * <xn y1>
     *                      t[2] =    gc^2  * <y2 y2>
     *                      t[3] = -2*gc    * <xn y2>
     *                      t[4] =  2*gp*gc * <y1 y2>
     *
     */

    /* determine the scaling exponent for g_code: ec = ec0 - 10 */
    exp_code = sub(exp_gcode0, 10);

    /* calculate exp_max[i] = s[i]-1 */
    exp_max[0] = sub(exp_coeff[0], 13);                        
    exp_max[1] = sub(exp_coeff[1], 14);                        
    exp_max[2] = add(exp_coeff[2], add(15, shl(exp_code, 1))); 
    exp_max[3] = add(exp_coeff[3], exp_code);                  
    exp_max[4] = add(exp_coeff[4], add(exp_code,1));           


    /*-------------------------------------------------------------------*
     *  Find maximum exponent:                                           *
     *  ~~~~~~~~~~~~~~~~~~~~~~                                           *
     *                                                                   *
     *  For the sum operation, all terms must have the same scaling;     *
     *  that scaling should be low enough to prevent cgOverflow. There-    *
     *  fore, the maximum scale is determined and all coefficients are   *
     *  re-scaled:                                                       *
     *                                                                   *
     *    e_max = max(exp_max[i]) + 1;                                   *
     *    e = exp_max[i]-e_max;         e <= 0!                          *
     *    c[i] = c[i]*2^e                                                *
     *-------------------------------------------------------------------*/

    e_max = exp_max[0];                                        
    for (i = 1; i < 5; i++)     /* implemented flattened */
    {
         
        if (sub(exp_max[i], e_max) > 0)
        {
            e_max = exp_max[i];                                
        }
    }

    e_max = add(e_max, 1);      /* To avoid cgOverflow */

    for (i = 0; i < 5; i++) {
        j = sub(e_max, exp_max[i]);
        L_tmp = L_deposit_h(frac_coeff[i]);
        L_tmp = L_shr(L_tmp, j);
        L_Extract(L_tmp, &coeff[i], &coeff_lo[i]);
    }


    /*-------------------------------------------------------------------*
     *  Codebook search:                                                 *
     *  ~~~~~~~~~~~~~~~~                                                 *
     *                                                                   *
     *  For each of the candiates LTP gains in g_pitch_cand[], the terms *
     *  t[0..4] are calculated from the values in the table (and the     *
     *  pitch gain candidate) and summed up; the result is the mean      *
     *  squared error for the LPT/CB gain pair. The index for the mini-  *
     *  mum MSE is stored and finally used to retrieve the quantized CB  *
     *  gain                                                             *
     *-------------------------------------------------------------------*/

    /* start with "infinite" MSE */
    dist_min = MAX_32;        
    cod_ind = 0;              
    pit_ind = 0;              

    /* loop through LTP gain candidates */
    for (j = 0; j < 3; j++)
    {
        /* pre-calculate terms only dependent on pitch gain */
        g_pitch = g_pitch_cand[j];    
        g2_pitch = mult(g_pitch, g_pitch);
        L_tmp0 = Mpy_32_16(        coeff[0], coeff_lo[0], g2_pitch);
        L_tmp0 = Mac_32_16(L_tmp0, coeff[1], coeff_lo[1], g_pitch);

        p = &qua_gain_code[0];
        for (i = 0; i < NB_QUA_CODE; i++)
        {
            g_code = *p++;         /* this is g_fac        Q11 */
            p++;                             /* skip log2(g_fac)         */
            p++;                             /* skip 20*log10(g_fac)     */

            g_code = mult(g_code, gcode0);

            L_tmp = L_mult (g_code, g_code);
            L_Extract (L_tmp, &g2_code_h, &g2_code_l);

            L_tmp = L_mult(g_code, g_pitch);
            L_Extract (L_tmp, &g_pit_cod_h, &g_pit_cod_l);

            L_tmp = Mac_32  (L_tmp0, coeff[2], coeff_lo[2],
                                     g2_code_h, g2_code_l);
            L_tmp = Mac_32_16(L_tmp, coeff[3], coeff_lo[3],
                                     g_code);
            L_tmp = Mac_32   (L_tmp, coeff[4], coeff_lo[4],
                                     g_pit_cod_h, g_pit_cod_l);

            /* store table index if MSE for this index is lower
               than the minimum MSE seen so far; also store the
               pitch gain for this (so far) lowest MSE          */
            
            if (L_sub(L_tmp, dist_min) < (Word32) 0)
            {
                dist_min = L_tmp;                
                cod_ind = i;                     
                pit_ind = j;                     
            }
        }
    }

    /*------------------------------------------------------------------*
     *  read quantized gains and new values for MA predictor memories   *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~   *
     *------------------------------------------------------------------*/

    /* Read the quantized gains */
    p = &qua_gain_code[add (add (cod_ind, cod_ind), cod_ind)]; 
    g_code = *p++;            
    *qua_ener_MR122 = *p++;   
    *qua_ener = *p;           

    /*------------------------------------------------------------------*
     *  calculate final fixed codebook gain:                            *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                            *
     *                                                                  *
     *   gc = gc0 * g                                                   *
     *------------------------------------------------------------------*/

    L_tmp = L_mult(g_code, gcode0);
    L_tmp = L_shr(L_tmp, sub(9, exp_gcode0));
    *gain_cod = extract_h(L_tmp);
    *gain_cod_ind = cod_ind;                
    *gain_pit = g_pitch_cand[pit_ind];      
    *gain_pit_ind = g_pitch_cind[pit_ind];  
}
Exemple #24
0
static Word16 D4i40_17_fast(/*(o) : Index of pulses positions.               */
  Word16 dn[],          /* (i)    : Correlations between h[] and Xn[].       */
  Word16 rr[],          /* (i)    : Correlations of impulse response h[].    */
  Word16 h[],           /* (i) Q12: Impulse response of filters.             */
  Word16 cod[],         /* (o) Q13: Selected algebraic codeword.             */
  Word16 y[],           /* (o) Q12: Filtered algebraic codeword.             */
  Word16 *sign          /* (o)    : Signs of 4 pulses.                       */
)
{
  Word16 i0, i1, i2, i3, ip0, ip1, ip2, ip3;
  Word16 i, j, ix, iy, track, trk, max;
  Word16 prev_i0, i1_offset;
  Word16 psk, ps, ps0, ps1, ps2, sq, sq2;
  Word16 alpk, alp, alp_16;
  Word32 s, alp0, alp1, alp2;
  Word16 *p0, *p1, *p2, *p3, *p4;
  Word16 sign_dn[L_SUBFR], sign_dn_inv[L_SUBFR], *psign;
  Word16 tmp_vect[NB_POS];
  Word16 *rri0i0, *rri1i1, *rri2i2, *rri3i3, *rri4i4;
  Word16 *rri0i1, *rri0i2, *rri0i3, *rri0i4;
  Word16 *rri1i2, *rri1i3, *rri1i4;
  Word16 *rri2i3, *rri2i4;

  Word16  *ptr_rri0i3_i4;
  Word16  *ptr_rri1i3_i4;
  Word16  *ptr_rri2i3_i4;
  Word16  *ptr_rri3i3_i4;

     /* Init pointers */
   rri0i0 = rr;
   rri1i1 = rri0i0 + NB_POS;
   rri2i2 = rri1i1 + NB_POS;
   rri3i3 = rri2i2 + NB_POS;
   rri4i4 = rri3i3 + NB_POS;
   rri0i1 = rri4i4 + NB_POS;
   rri0i2 = rri0i1 + MSIZE;
   rri0i3 = rri0i2 + MSIZE;
   rri0i4 = rri0i3 + MSIZE;
   rri1i2 = rri0i4 + MSIZE;
   rri1i3 = rri1i2 + MSIZE;
   rri1i4 = rri1i3 + MSIZE;
   rri2i3 = rri1i4 + MSIZE;
   rri2i4 = rri2i3 + MSIZE;

 /*-----------------------------------------------------------------------*
  * Chose the sign of the impulse.                                        *
  *-----------------------------------------------------------------------*/

   for (i=0; i<L_SUBFR; i++)
   {
     if (dn[i] >= 0)
     {
       sign_dn[i] = MAX_16;
       sign_dn_inv[i] = MIN_16;
     }
     else
     {
       sign_dn[i] = MIN_16;
       sign_dn_inv[i] = MAX_16;
       dn[i] = negate(dn[i]);
     }
   }

 /*-------------------------------------------------------------------*
  * Modification of rrixiy[] to take signs into account.              *
  *-------------------------------------------------------------------*/

  p0 = rri0i1;
  p1 = rri0i2;
  p2 = rri0i3;
  p3 = rri0i4;

  for(i0=0; i0<L_SUBFR; i0+=STEP)
  {
    psign = sign_dn;
    if (psign[i0] < 0) psign = sign_dn_inv;

    for(i1=1; i1<L_SUBFR; i1+=STEP)
    {
      *p0++ = mult(*p0, psign[i1]);
      *p1++ = mult(*p1, psign[i1+1]);
      *p2++ = mult(*p2, psign[i1+2]);
      *p3++ = mult(*p3, psign[i1+3]);
    }
  }

  p0 = rri1i2;
  p1 = rri1i3;
  p2 = rri1i4;

  for(i1=1; i1<L_SUBFR; i1+=STEP)
  {
    psign = sign_dn;
    if (psign[i1] < 0) psign = sign_dn_inv;

    for(i2=2; i2<L_SUBFR; i2+=STEP)
    {
      *p0++ = mult(*p0, psign[i2]);
      *p1++ = mult(*p1, psign[i2+1]);
      *p2++ = mult(*p2, psign[i2+2]);
    }
  }

  p0 = rri2i3;
  p1 = rri2i4;

  for(i2=2; i2<L_SUBFR; i2+=STEP)
  {
    psign = sign_dn;
    if (psign[i2] < 0) psign = sign_dn_inv;

    for(i3=3; i3<L_SUBFR; i3+=STEP)
    {
      *p0++ = mult(*p0, psign[i3]);
      *p1++ = mult(*p1, psign[i3+1]);
    }
  }


 /*-------------------------------------------------------------------*
  * Search the optimum positions of the four pulses which maximize    *
  *     square(correlation) / energy                                  *
  *-------------------------------------------------------------------*/

  psk = -1;
  alpk = 1;

  ptr_rri0i3_i4 = rri0i3;
  ptr_rri1i3_i4 = rri1i3;
  ptr_rri2i3_i4 = rri2i3;
  ptr_rri3i3_i4 = rri3i3;

  /* Initializations only to remove warning from some compilers */

  ip0=0; ip1=1; ip2=2; ip3=3; ix=0; iy=0; ps=0;

  /* search 2 times: track 3 and 4 */
  for (track=3, trk=0; track<5; track++, trk++)
  {
   /*------------------------------------------------------------------*
    * depth first search 3, phase A: track 2 and 3/4.                  *
    *------------------------------------------------------------------*/

    sq = -1;
    alp = 1;

    /* i0 loop: 2 positions in track 2 */

    prev_i0  = -1;

    for (i=0; i<2; i++)
    {
      max = -1;
      /* search "dn[]" maximum position in track 2 */
      for (j=2; j<L_SUBFR; j+=STEP)
      {
        if ((sub(dn[j], max) > 0) && (sub(prev_i0,j) != 0))
        {
          max = dn[j];
          i0 = j;
        }
      }
      prev_i0 = i0;

      j = mult(i0, 6554);        /* j = i0/5 */
      p0 = rri2i2 + j;

      ps1 = dn[i0];
      alp1 = L_mult(*p0, _1_4);

      /* i1 loop: 8 positions in track 2 */

      p0 = ptr_rri2i3_i4 + shl(j, 3);
      p1 = ptr_rri3i3_i4;

      for (i1=track; i1<L_SUBFR; i1+=STEP)
      {
        ps2 = add(ps1, dn[i1]);       /* index increment = STEP */

        /* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */
        alp2 = L_mac(alp1, *p0++, _1_2);
        alp2 = L_mac(alp2, *p1++, _1_4);

        sq2 = mult(ps2, ps2);
        alp_16 = round(alp2);

        s = L_msu(L_mult(alp,sq2),sq,alp_16);
        if (s > 0)
        {
          sq = sq2;
          ps = ps2;
          alp = alp_16;
          ix = i0;
          iy = i1;
        }
      }
    }

    i0 = ix;
    i1 = iy;
    i1_offset = shl(mult(i1, 6554), 3);       /* j = 8*(i1/5) */

   /*------------------------------------------------------------------*
    * depth first search 3, phase B: track 0 and 1.                    *
    *------------------------------------------------------------------*/

    ps0 = ps;
    alp0 = L_mult(alp, _1_4);

    sq = -1;
    alp = 1;

    /* build vector for next loop to decrease complexity */

    p0 = rri1i2 + mult(i0, 6554);
    p1 = ptr_rri1i3_i4 + mult(i1, 6554);
    p2 = rri1i1;
    p3 = tmp_vect;

    for (i3=1; i3<L_SUBFR; i3+=STEP)
    {
      /* rrv[i3] = rr[i3][i3] + rr[i0][i3] + rr[i1][i3]; */
      s = L_mult(*p0, _1_4);        p0 += NB_POS;
      s = L_mac(s, *p1, _1_4);      p1 += NB_POS;
      s = L_mac(s, *p2++, _1_8);
      *p3++ = round(s);
    }

    /* i2 loop: 8 positions in track 0 */

    p0 = rri0i2 + mult(i0, 6554);
    p1 = ptr_rri0i3_i4 + mult(i1, 6554);
    p2 = rri0i0;
    p3 = rri0i1;

    for (i2=0; i2<L_SUBFR; i2+=STEP)
    {
      ps1 = add(ps0, dn[i2]);         /* index increment = STEP */

      /* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */
      alp1 = L_mac(alp0, *p0, _1_8);       p0 += NB_POS;
      alp1 = L_mac(alp1, *p1, _1_8);       p1 += NB_POS;
      alp1 = L_mac(alp1, *p2++, _1_16);

      /* i3 loop: 8 positions in track 1 */

      p4 = tmp_vect;

      for (i3=1; i3<L_SUBFR; i3+=STEP)
      {
        ps2 = add(ps1, dn[i3]);       /* index increment = STEP */

        /* alp1 = alp0 + rr[i0][i3] + rr[i1][i3] + rr[i2][i3] + 1/2*rr[i3][i3]; */
        alp2 = L_mac(alp1, *p3++, _1_8);
        alp2 = L_mac(alp2, *p4++, _1_2);

        sq2 = mult(ps2, ps2);
        alp_16 = round(alp2);

        s = L_msu(L_mult(alp,sq2),sq,alp_16);
        if (s > 0)
        {
          sq = sq2;
          alp = alp_16;
          ix = i2;
          iy = i3;
        }
      }
    }

   /*----------------------------------------------------------------*
    * depth first search 3: compare codevector with the best case.   *
    *----------------------------------------------------------------*/

    s = L_msu(L_mult(alpk,sq),psk,alp);
    if (s > 0)
    {
      psk = sq;
      alpk = alp;
      ip2 = i0;
      ip3 = i1;
      ip0 = ix;
      ip1 = iy;
    }

   /*------------------------------------------------------------------*
    * depth first search 4, phase A: track 3 and 0.                    *
    *------------------------------------------------------------------*/

    sq = -1;
    alp = 1;

    /* i0 loop: 2 positions in track 3/4 */

    prev_i0  = -1;

    for (i=0; i<2; i++)
    {
      max = -1;
      /* search "dn[]" maximum position in track 3/4 */
      for (j=track; j<L_SUBFR; j+=STEP)
      {
        if ((sub(dn[j], max) > 0) && (sub(prev_i0,j) != 0))
        {
          max = dn[j];
          i0 = j;
        }
      }
      prev_i0 = i0;

      j = mult(i0, 6554);        /* j = i0/5 */
      p0 = ptr_rri3i3_i4 + j;

      ps1 = dn[i0];
      alp1 = L_mult(*p0, _1_4);

      /* i1 loop: 8 positions in track 0 */

      p0 = ptr_rri0i3_i4 + j;
      p1 = rri0i0;

      for (i1=0; i1<L_SUBFR; i1+=STEP)
      {
        ps2 = add(ps1, dn[i1]);       /* index increment = STEP */

        /* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */
        alp2 = L_mac(alp1, *p0, _1_2);       p0 += NB_POS;
        alp2 = L_mac(alp2, *p1++, _1_4);

        sq2 = mult(ps2, ps2);
        alp_16 = round(alp2);

        s = L_msu(L_mult(alp,sq2),sq,alp_16);
        if (s > 0)
        {
          sq = sq2;
          ps = ps2;
          alp = alp_16;
          ix = i0;
          iy = i1;
        }
      }
    }

    i0 = ix;
    i1 = iy;
    i1_offset = shl(mult(i1, 6554), 3);       /* j = 8*(i1/5) */

   /*------------------------------------------------------------------*
    * depth first search 4, phase B: track 1 and 2.                    *
    *------------------------------------------------------------------*/

    ps0 = ps;
    alp0 = L_mult(alp, _1_4);

    sq = -1;
    alp = 1;

    /* build vector for next loop to decrease complexity */

    p0 = ptr_rri2i3_i4 + mult(i0, 6554);
    p1 = rri0i2 + i1_offset;
    p2 = rri2i2;
    p3 = tmp_vect;

    for (i3=2; i3<L_SUBFR; i3+=STEP)
    {
      /* rrv[i3] = rr[i3][i3] + rr[i0][i3] + rr[i1][i3]; */
      s = L_mult(*p0, _1_4);         p0 += NB_POS;
      s = L_mac(s, *p1++, _1_4);
      s = L_mac(s, *p2++, _1_8);
      *p3++ = round(s);
    }

    /* i2 loop: 8 positions in track 1 */

    p0 = ptr_rri1i3_i4 + mult(i0, 6554);
    p1 = rri0i1 + i1_offset;
    p2 = rri1i1;
    p3 = rri1i2;

    for (i2=1; i2<L_SUBFR; i2+=STEP)
    {
      ps1 = add(ps0, dn[i2]);         /* index increment = STEP */

      /* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */
      alp1 = L_mac(alp0, *p0, _1_8);       p0 += NB_POS;
      alp1 = L_mac(alp1, *p1++, _1_8);
      alp1 = L_mac(alp1, *p2++, _1_16);

      /* i3 loop: 8 positions in track 2 */

      p4 = tmp_vect;

      for (i3=2; i3<L_SUBFR; i3+=STEP)
      {
        ps2 = add(ps1, dn[i3]);       /* index increment = STEP */

        /* alp1 = alp0 + rr[i0][i3] + rr[i1][i3] + rr[i2][i3] + 1/2*rr[i3][i3]; */
        alp2 = L_mac(alp1, *p3++, _1_8);
        alp2 = L_mac(alp2, *p4++, _1_2);

        sq2 = mult(ps2, ps2);
        alp_16 = round(alp2);

        s = L_msu(L_mult(alp,sq2),sq,alp_16);
        if (s > 0)
        {
          sq = sq2;
          alp = alp_16;
          ix = i2;
          iy = i3;
        }
      }
    }

   /*----------------------------------------------------------------*
    * depth first search 1: compare codevector with the best case.   *
    *----------------------------------------------------------------*/

    s = L_msu(L_mult(alpk,sq),psk,alp);
    if (s > 0)
    {
      psk = sq;
      alpk = alp;
      ip3 = i0;
      ip0 = i1;
      ip1 = ix;
      ip2 = iy;
    }

  ptr_rri0i3_i4 = rri0i4;
  ptr_rri1i3_i4 = rri1i4;
  ptr_rri2i3_i4 = rri2i4;
  ptr_rri3i3_i4 = rri4i4;

  }


 /* Set the sign of impulses */

 i0 = sign_dn[ip0];
 i1 = sign_dn[ip1];
 i2 = sign_dn[ip2];
 i3 = sign_dn[ip3];

 /* Find the codeword corresponding to the selected positions */


 for(i=0; i<L_SUBFR; i++) {
   cod[i] = 0;
 }

 cod[ip0] = shr(i0, 2);         /* From Q15 to Q13 */
 cod[ip1] = shr(i1, 2);
 cod[ip2] = shr(i2, 2);
 cod[ip3] = shr(i3, 2);

 /* find the filtered codeword */

 for (i = 0; i < ip0; i++) y[i] = 0;

 if(i0 > 0)
   for(i=ip0, j=0; i<L_SUBFR; i++, j++) y[i] = h[j];
 else
   for(i=ip0, j=0; i<L_SUBFR; i++, j++) y[i] = negate(h[j]);

 if(i1 > 0)
   for(i=ip1, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]);
 else
   for(i=ip1, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]);

 if(i2 > 0)
   for(i=ip2, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]);
 else
   for(i=ip2, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]);

 if(i3 > 0)
   for(i=ip3, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]);
 else
   for(i=ip3, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]);

 /* find codebook index;  17-bit address */

 i = 0;
 if(i0 > 0) i = add(i, 1);
 if(i1 > 0) i = add(i, 2);
 if(i2 > 0) i = add(i, 4);
 if(i3 > 0) i = add(i, 8);
 *sign = i;

 ip0 = mult(ip0, 6554);         /* ip0/5 */
 ip1 = mult(ip1, 6554);         /* ip1/5 */
 ip2 = mult(ip2, 6554);         /* ip2/5 */
 i   = mult(ip3, 6554);         /* ip3/5 */
 j   = add(i, shl(i, 2));       /* j = i*5 */
 j   = sub(ip3, add(j, 3));     /* j= ip3%5 -3 */
 ip3 = add(shl(i, 1), j);

 i = add(ip0, shl(ip1, 3));
 i = add(i  , shl(ip2, 6));
 i = add(i  , shl(ip3, 9));

 return i;
}
Exemple #25
0
inline ivect mult(imat M, ivect v) {
    return matc2vec(mult(M, vec2matc(v)), 0);
}
Exemple #26
0
static int random(int seed)
{
  int tmp;
  tmp = (mult(seed,CONST_b)+1);
  return tmp;
}
void UART2_ProcessSpektrumData( )
{
    // PROCESS Spektrum receiver data -- see http://www.desertrc.com/spektrum_protocol.htm for protocol
    // Determine what to do with received character
    
    uint8_t i;
    int16_t scaleInt;
    _Q16 tmpQ16, scaleQ16, scale2Q16;
    // Data is 14 bytes long
    for( i=0; i<14; i+=2)
    {

        uint16_t rcdata = spektrumData[i] << 8; // MSB
        rcdata += spektrumData[i+1];	// LSB
#ifdef DSMX
        // 11-bit data mode
        uint16_t cmddata = (int16_t) (rcdata & 0b0000011111111111); // get last 11 bits
        uint8_t channel = rcdata >> 11; // get 5 first bits
        scaleQ16 = num1024;
        scale2Q16 = num2048;
        scaleInt = 8;
#else
        // 10-bit data mode
        uint16_t cmddata = (int16_t) (rcdata & 0b0000001111111111); // get last 10 bits
        uint8_t channel = rcdata >> 10; // get 6 first bits
        scaleQ16 = num512;
        scale2Q16 = num1024;
        scaleInt = 4;
#endif


        switch(channel){	// process channel data
            case 0:
                RCdata.ch0 = cmddata;
                break;
            case 1:
                int16toQ16(&tmpQ16,&cmddata);
                tmpQ16 -= scaleQ16;
                RCdata.ch1 = _IQ16div(tmpQ16,scale2Q16);
                break;
            case 2:
                int16toQ16(&tmpQ16,&cmddata);
                tmpQ16 -= scaleQ16;
                RCdata.ch2 = _IQ16div(tmpQ16,scale2Q16);
                break;
            case 3:
                int16toQ16(&tmpQ16,&cmddata);
                tmpQ16 -= scaleQ16;
                RCdata.ch3 = _IQ16div(tmpQ16,scaleQ16);
                break;
            case 4:
                RCdata.ch4 = cmddata;
                break;
            case 5:
                RCdata.ch5 = cmddata;
                break;
            case 6:
                RCdata.ch6 = cmddata;
                break;
            default:
                break;
        } // End switch channel
    } // End for each data byte

    // manual mode
    if (RCdata.ch4 > 600) {
        CmdData.AttCmd = 1;
        //Debug: Scaling throttle down 2 times
        //CmdData.throttle = RCdata.ch0/(scaleInt*2);  // RCdata is between 0 and 1023, this will be approximately between 0 and 255
        //Normal throttle
        CmdData.throttle = RCdata.ch0/(scaleInt);  // RCdata is between 0 and 1023, this will be approximately between 0 and 255
        _Q16 halfRoll = -RCdata.ch1; //mult(RCdata.ch1,num0p5);  // this is about -0.6 -> 0.6  which is +/- ~70 degrees
        _Q16 halfPitch = RCdata.ch2; //mult(RCdata.ch2,num0p5);
        _Q16 cosRoll = _Q16cos(halfRoll);
        _Q16 sinRoll = _Q16sin(halfRoll);
        _Q16 cosPitch = _Q16cos(halfPitch);
        _Q16 sinPitch = _Q16sin(halfPitch);
        CmdData.q_cmd.o = mult(cosRoll,cosPitch);
        CmdData.q_cmd.x = mult(sinRoll,cosPitch);
        CmdData.q_cmd.y = mult(cosRoll,sinPitch);
        CmdData.q_cmd.z = -mult(sinRoll,sinPitch);
        CmdData.p = CmdData.q = 0;
        CmdData.r = mult(RCdata.ch3,num2p0);

        // Turn on green LED to signify manual control mode ON
        led_on(LED_GREEN);
    } else
    {
        // Turn off green LED to signify manual control mode OFF
        led_off(LED_GREEN);
        CmdData.AttCmd = 0;
    }

}
Exemple #28
0
Node Node::operator*(const IntLeaf &leaf) const {
	return mult(leaf);	
}
void prim_conv11::operator()
  (
   signal_& quality__io,
   signal_& wiregroup__io,
   signal_& hstrip__io,
   signal_& clctpat__io,
   signal_& ph__io,
   signal_& th__io,
   signal_& vl__io,
   signal_& phzvl__io,
   signal_& me11a__io,
   signal_& clctpat_r__io,
   signal_& ph_hit__io,
   signal_& th_hit__io,
   signal_& sel__io,
   signal_& addr__io,
   signal_& r_in__io,
   signal_& r_out__io,
   signal_& we__io,
   signal_& clk__io,
   signal_& control_clk__io
   )
{
  if (!built)
    {
      seg_ch = 2;
      bw_ph = 8;
      bw_th = 7;
      bw_fph = 12;
      bw_fth = 8;
      bw_wg = 7;
      bw_ds = 7;
      bw_hs = 8;
      pat_w_st3 = 3;
      pat_w_st1 = pat_w_st3 + 1;
      full_pat_w_st3 = (1 << (pat_w_st3+1)) - 1;
      full_pat_w_st1 = (1 << (pat_w_st1+1)) - 1;
      padding_w_st1 = full_pat_w_st1 / 2;
      padding_w_st3 = full_pat_w_st3 / 2;
      red_pat_w_st3 = pat_w_st3 * 2 + 1;
      red_pat_w_st1 = pat_w_st1 * 2 + 1;
      fold = 4;
      th_ch11 = seg_ch*seg_ch;
      bw_q = 4;
      bw_addr = 7;
      ph_raw_w = (1 << pat_w_st3) * 15;
      th_raw_w = (1 << bw_th);
      max_drift = 3;
      bw_phi = 12;
      bw_eta = 7;
      ph_hit_w = 40+4;
      ph_hit_w20 = ph_hit_w;
      ph_hit_w10 = 20+4;
      th_hit_w = 56 + 8;
      endcap = 1;
      n_strips = (station <= 1 && cscid <= 2) ? 64 :
	(station <= 1 && cscid >= 6) ? 64 : 80;
      n_wg = (station <= 1 && cscid <= 3) ? 48  :
	(station <= 1 && cscid >= 6) ? 32  :
	(station == 2 && cscid <= 3) ? 112 :
	(station >= 3 && cscid <= 3) ? 96  : 64;
      th_coverage = (station <= 1 && cscid <= 2) ? 45  :
	(station <= 1 && cscid >= 6) ? 27  :
	(station <= 1 && cscid >= 3) ? 39  :
	(station == 2 && cscid <= 2) ? 43  :
	(station == 2 && cscid >= 3) ? 56  :
	(station == 3 && cscid <= 2) ? 34  :
	(station == 3 && cscid >= 3) ? 52  :
	(station == 4 && cscid <= 2) ? 28  :
	(station == 4 && cscid >= 3) ? 50  : 0;
      ph_coverage = (station <= 1 && cscid >= 6) ? 15 : //30 :
	(station >= 2 && cscid <= 2) ? 40 : 20;
      th_ch = (station <= 1 && cscid <= 2) ? (seg_ch*seg_ch) : seg_ch;
      ph_reverse = (endcap == 1 && station >= 3) ? 1 : 
	(endcap == 2 && station <  3) ? 1 : 0;
      th_mem_sz = (1 << bw_addr);
      th_corr_mem_sz = (1 << bw_addr);
      mult_bw = bw_fph + 11;
      ph_zone_bnd1 = (station <= 1 && cscid <= 2) ? 41 :
	(station == 2 && cscid <= 2) ? 41 :
	(station == 2 && cscid >  2) ? 87 :
	(station == 3 && cscid >  2) ? 49 :
	(station == 4 && cscid >  2) ? 49 : 127;
      ph_zone_bnd2 = (station == 3 && cscid >  2) ? 87 : 127;
      zone_overlap = 2;
      bwr = 6;
      bpow = 6;
      cnr = (1 << bpow);
      cnrex = ph_raw_w;
      build();
      // input parameters from MPC
      quality.attach(quality__io);
      wiregroup.attach(wiregroup__io);
      hstrip.attach(hstrip__io);
      clctpat.attach(clctpat__io);
      sel.attach(sel__io);
      addr.attach(addr__io);
      r_in.attach(r_in__io);
      we.attach(we__io);
      clk.attach(clk__io);
      control_clk.attach(control_clk__io);
      // outputs
      // low-precision ph, only for detection
      // high-precision ph with displacement correction will be calculated when 
      // 3 best tracks are found.
      ph.attach(ph__io);
      // full precision th, but without displacement correction, takes th duplication into account
      th.attach(th__io);
      // one-bit valid flags
      vl.attach(vl__io);
      phzvl.attach(phzvl__io);
      me11a.attach(me11a__io);
      clctpat_r.attach(clctpat_r__io);
      // ph and th raw hits
      ph_hit.attach(ph_hit__io);
      th_hit.attach(th_hit__io);
      r_out.attach(r_out__io);
    }

	
  pc_id(3,0) = cscid;
  pc_id(7,4) = station;
	
  r_out = (sel == const_(2, 0x0UL)) ? params[addr] : 
    (sel == const_(2, 0x1UL)) ? th_mem[addr] : 
    (sel == const_(2, 0x2UL)) ? th_corr_mem[addr] : pc_id;

  beginalways();	

  if (posedge (control_clk))
    {
      if (( (sel) == 0)) {  { if (we) params     [addr] = r_in; } } else 
	if (( (sel) == 1)) {  { if (we) th_mem     [addr] = r_in; } } else 
	  if (( (sel) == 2)) {  { if (we) th_corr_mem[addr] = r_in(3,0); } } 
    }
  endalways();

  beginalways();
	
  if (posedge (clk))
    {

      // zero outputs
      vl = 0;
      phzvl = 0;
      for (i = 0; i < seg_ch; i = i+1) { fph[i] = 0; clctpat_r[i] = 0; }
      for (i = 0; i < th_ch;  i = i+1) th[i] = 0;
      ph_hit = 0;
      th_hit = 0;

      for (i = 0; i < seg_ch; i = i+1)
	{

	  factor[i] = (station <= 1 && cscid <= 2 && hstrip[i] > 127) ? 1707 : // ME1/1a
	    1301; // ME1/1b
				 
	  //if(factor[i])
	  //	std::cout<<"factor 11 = "<<factor[i]<<std::endl;

	  me11a_w[i] = (station <= 1 && cscid <= 2 && hstrip[i] > 127);
	  if (( (clctpat[i]) == 0)) {  { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else 
	    if (( (clctpat[i]) == 1)) {  { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else 
	      if (( (clctpat[i]) == 2)) {  { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 1; } } else 
		if (( (clctpat[i]) == 3)) {  { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 0; } } else 
		  if (( (clctpat[i]) == 4)) {  { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 1; } } else 
		    if (( (clctpat[i]) == 5)) {  { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 0; } } else 
		      if (( (clctpat[i]) == 6)) {  { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 1; } } else 
			if (( (clctpat[i]) == 7)) {  { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 0; } } else 
			  if (( (clctpat[i]) == 8)) {  { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 1; } } else 
			    if (( (clctpat[i]) == 9)) {  { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 0; } } else 
			      if (( (clctpat[i]) == 10)) {  { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else  {  { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } 

	  // reverse clct pattern correction if chamber is reversed
	  //			if (ph_reverse) clct_pat_sign = ~clct_pat_sign;

	  // convert into 1/8 strips and remove ME1/1a offset (512=128*4)
	  eight_str[i]  = (const_s(2, 0x0UL), hstrip [i], const_s(2, 0x0UL)) - (me11a_w[i] ? 512 : 0);
			
	  // clct pattern correction
	  if (clct_pat_sign == 0) eight_str[i] = eight_str[i] + clct_pat_corr(2,1);
	  else eight_str[i] = eight_str[i] - clct_pat_corr(2,1);

	  if (quality[i])
	    {
	      vl[i] = 1;
	      // ph conversion
	      // for factors 1024 and 2048 the multiplier should be replaced with shifts by synthesizer
	      mult = eight_str[i] * factor[i];
	      ph_tmp = mult(mult_bw-1 , 10);
	      //std::cout<<"ph_tmp = "<<ph_tmp<<std::endl;
	      ph_init_ix =  me11a_w[i] ? const_(3, 2UL) : const_(3, 0UL); // index of ph_init parameter to apply (different for ME11a and b)
	      //std::cout<<"ph_init_ix = "<<ph_init_ix<<std::endl;
	      if (ph_reverse)
		{
		  fph[i] = params[ph_init_ix] - ph_tmp;
		  // set ph raw hits
		  ph_hit[ph_coverage - ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)] = 1;
		}
	      else
		{            
		  fph[i] = params[ph_init_ix] + ph_tmp;
		  // set ph raw hits
		  ph_hit[ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)] = 1;
		  /* -----\/----- EXCLUDED -----\/-----
		  // add hits to take ME11a strip ganging into account
		  // offsets of 14 and 28 is what I observe from MC. Calculations show 11.6 and 23.2 (???)
		  if (me11a_w[i])
		  {
		  ph_hit[ph_tmp(bw_fph-1,5) + params[2] + 14] = 1;
		  ph_hit[ph_tmp(bw_fph-1,5) + params[2] + 28] = 1;
		  }
		  -----/\----- EXCLUDED -----/\----- */
		}
				
	      //std::cout<<"estr = "<<eight_str[i]<<", factor = "<<factor[i]<<", ph_rev = "<<ph_reverse<<", ph_tmp = "<<ph_tmp<<std::endl;
	      //std::cout<<"ph_hit = "<<ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)<<std::endl;
	      //std::cout<<"strip = "<<hstrip[i]<<", Id = "<<cscid + 1<<",phinit = "<<(params[ph_init_ix])<<"ph_cov = "<<ph_coverage<<", and phshift = "<<(ph_tmp(bw_fph-1,5))<<", and phdisp = "<<(params[ph_init_ix + const_(3, 1UL)](7,1))<<"\n\n\n";
				
				
	      wg = wiregroup[i];
	      // th conversion
	      // call appropriate LUT, it returns th[i] relative to wg0 of that chamber
	      th_orig = th_mem[wg];
	      //std::cout<<"wire = "<<wg<<", th_mem[wg] = "<<th_orig<<std::endl;


	      // need th duplication here
	      for (j = 0; j < seg_ch; j = j+1)
		{
		  if (quality[j])
		    {
		      // calculate correction for each strip number
		      // index is: (wiregroup(2 MS bits), dblstrip(5-bit for these chambers))
		      index = (wg(5,4), eight_str[j](8,4));
		      th_corr = th_corr_mem[index];
		      //std::cout<<"eightstrip = "<<eight_str[j]<<", eightstrip(8,4) = "<<eight_str[j](8,4)<<", index = "<<index<<", th_corr = "<<th_corr<<std::endl;

		      //std::cout<<"th_corr = "<<th_corr<<" and th_orig = "<<th_orig<<std::endl;
		      // apply correction to the corresponding output
		      if (ph_reverse) th_tmp = (th_orig - th_corr) & const_(6, 0x3fUL);
		      else            th_tmp = (th_orig + th_corr) & const_(6, 0x3fUL);
		      //std::cout<<"ph_reverse = "<<ph_reverse<<" ";
		      //if(th_tmp)std::cout<<"th_tmp = "<<th_tmp<<" and thcoverage = "<<th_coverage<<std::endl;
		      // check that correction did not make invalid value outside chamber coverage
		      // this will actually take care of both positive and negative illegal values
		      if (th_tmp < th_coverage) 
			{
			  // apply initial th value for that chamber
			  th[i*seg_ch+j] = th_tmp + params[4];
			  //std::cout<<"params[4] = "<<params[4]<<"\n";
							
			  // th hits
			  th_hit[th_tmp + params[5]] = 1;
							
							
			  // check which zones ph hits should be applied to
			  if (th[i*seg_ch+j] <= (ph_zone_bnd1 + zone_overlap)) phzvl[0] = 1;
			  if (th[i*seg_ch+j] >  (ph_zone_bnd2 - zone_overlap)) phzvl[2] = 1;
			  if (
			      (th[i*seg_ch+j] >  (ph_zone_bnd1 - zone_overlap)) &&
			      (th[i*seg_ch+j] <= (ph_zone_bnd2 + zone_overlap))
			      ) phzvl[1] = 1;
			  //std::cout<<"ph_zone_bnd1 = "<<ph_zone_bnd1<<std::endl;
			  //std::cout<<"phzvl = "<<phzvl<<std::endl;
			}
		    }
		}
	      clctpat_r[i] = clctpat[i]; // just propagate pattern downstream
	    } // if (quality[i])
			
	  ph[i] = fph[i];
	  //if(fph[i]) std::cout<<"fph["<<i<<"] = "<<fph[i]<<" and vl[i] = "<<vl[i]<<std::endl;
			
	} // for (i = 0; i < seg_ch; i = i+1)
      me11a = me11a_w;
    }
  endalways();
}
Exemple #30
0
void WebRtcG729fix_Decod_ld8a(
  Decod_ld8a_state *st,
  int16_t  parm[],      /* (i)   : vector of synthesis parameters
                                  parm[0] = bad frame indicator (bfi)   */
  int16_t  synth[],     /* (o)   : synthesis speech                     */
  int16_t  A_t[],       /* (o)   : decoded LP filter in 2 subframes     */
  int16_t  *T2,         /* (o)   : decoded pitch lag in 2 subframes     */
  int16_t  *Vad,        /* (o)   : frame type                           */
  int16_t  bad_lsf      /* (i)   : bad LSF indicator                    */
)
{
  int16_t  *Az;                  /* Pointer on A_t   */
  int16_t  lsp_new[M];           /* LSPs             */
  int16_t  code[L_SUBFR];        /* ACELP codevector */

  /* Scalars */

  int16_t  i, j, i_subfr;
  int16_t  T0, T0_frac, index;
  int16_t  bfi;
  int32_t  L_temp;

  int16_t bad_pitch;             /* bad pitch indicator */

  /* for G.729B */
  int16_t ftyp;
  int16_t lsfq_mem[MA_NP][M];

  int Overflow;

  /* Test bad frame indicator (bfi) */

  bfi = *parm++;
  /* for G.729B */
  ftyp = *parm;

  if(bfi == 1) {
    if(st->past_ftyp == 1) {
      ftyp = 1;
      parm[4] = 1;    /* G.729 maintenance */
    }
    else ftyp = 0;
    *parm = ftyp;  /* modification introduced in version V1.3 */
  }
  
  *Vad = ftyp;

  /* Processing non active frames (SID & not transmitted) */
  if(ftyp != 1) {
    
    WebRtcG729fix_Get_decfreq_prev(st, lsfq_mem);
    WebRtcG729fix_Dec_cng(st, st->past_ftyp, st->sid_sav, st->sh_sid_sav,
            parm, st->exc, st->lsp_old, A_t, &st->seed, lsfq_mem);
    WebRtcG729fix_Update_decfreq_prev(st, lsfq_mem);

    Az = A_t;
    for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
      Overflow = WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 0);
      if(Overflow != 0) {
        /* In case of overflow in the synthesis          */
        /* -> Scale down vector exc[] and redo synthesis */
        
        for (i = 0; i < PIT_MAX + L_INTERPOL + L_FRAME; i++)
          st->old_exc[i] = shr(st->old_exc[i], 2);
        
        WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 1);
      }
      else
        WEBRTC_SPL_MEMCPY_W16(st->mem_syn, &synth[i_subfr+L_SUBFR-M], M);
      
      Az += MP1;

      *T2++ = st->old_T0;
    }
    st->sharp = SHARPMIN;
    
  }
  /* Processing active frame */
  else {
    
    st->seed = INIT_SEED;
    parm++;

    /* Decode the LSPs */
    
    WebRtcG729fix_D_lsp(st, parm, lsp_new, WebRtcSpl_AddSatW16(bfi, bad_lsf));
    parm += 2;
    
    /*
       Note: "bad_lsf" is introduce in case the standard is used with
       channel protection.
       */
    
    /* Interpolation of LPC for the 2 subframes */
    
    WebRtcG729fix_Int_qlpc(st->lsp_old, lsp_new, A_t);
    
    /* update the LSFs for the next frame */
    
    WEBRTC_SPL_MEMCPY_W16(st->lsp_old, lsp_new, M);
    
    /*------------------------------------------------------------------------*
     *          Loop for every subframe in the analysis frame                 *
     *------------------------------------------------------------------------*
     * The subframe size is L_SUBFR and the loop is repeated L_FRAME/L_SUBFR  *
     *  times                                                                 *
     *     - decode the pitch delay                                           *
     *     - decode algebraic code                                            *
     *     - decode pitch and codebook gains                                  *
     *     - find the excitation and compute synthesis speech                 *
     *------------------------------------------------------------------------*/
    
    Az = A_t;            /* pointer to interpolated LPC parameters */
    
    for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
      {

        index = *parm++;        /* pitch index */

        if(i_subfr == 0)
          {
            i = *parm++;        /* get parity check result */
            bad_pitch = WebRtcSpl_AddSatW16(bfi, i);
            if( bad_pitch == 0)
              {
                WebRtcG729fix_Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
                st->old_T0 = T0;
              }
            else                /* Bad frame, or parity error */
              {
                T0  = st->old_T0;
                T0_frac = 0;
                st->old_T0 = WebRtcSpl_AddSatW16(st->old_T0, 1);
                if(st->old_T0 > PIT_MAX) {
                  st->old_T0 = PIT_MAX;
                }
              }
          }
        else                    /* second subframe */
          {
            if(bfi == 0)
              {
                WebRtcG729fix_Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
                st->old_T0 = T0;
              }
            else
              {
                T0 = st->old_T0;
                T0_frac = 0;
                st->old_T0 = WebRtcSpl_AddSatW16(st->old_T0, 1);
                if (st->old_T0 > PIT_MAX) {
                  st->old_T0 = PIT_MAX;
                }
              }
          }
        *T2++ = T0;

        /*-------------------------------------------------*
         * - Find the adaptive codebook vector.            *
         *-------------------------------------------------*/

        WebRtcG729fix_Pred_lt_3(&st->exc[i_subfr], T0, T0_frac, L_SUBFR);

        /*-------------------------------------------------------*
         * - Decode innovative codebook.                         *
         * - Add the fixed-gain pitch contribution to code[].    *
         *-------------------------------------------------------*/

        if(bfi != 0)            /* Bad frame */
          {

            parm[0] = WebRtcG729fix_Random(&st->seed_fer) & (int16_t)0x1fff; /* 13 bits random */
            parm[1] = WebRtcG729fix_Random(&st->seed_fer) & (int16_t)0x000f; /*  4 bits random */
          }

        WebRtcG729fix_Decod_ACELP(parm[1], parm[0], code);
        parm +=2;

        j = shl(st->sharp, 1);      /* From Q14 to Q15 */
        if(T0 < L_SUBFR) {
          for (i = T0; i < L_SUBFR; i++) {
            code[i] = WebRtcSpl_AddSatW16(code[i], mult(code[i-T0], j));
          }
        }

        /*-------------------------------------------------*
         * - Decode pitch and codebook gains.              *
         *-------------------------------------------------*/

        index = *parm++;        /* index of energy VQ */

        WebRtcG729fix_Dec_gain(st, index, code, L_SUBFR, bfi, &st->gain_pitch, &st->gain_code);

        /*-------------------------------------------------------------*
         * - Update pitch sharpening "sharp" with quantized gain_pitch *
         *-------------------------------------------------------------*/

        st->sharp = st->gain_pitch;
        if (st->sharp > SHARPMAX)      { st->sharp = SHARPMAX; }
        else if (st->sharp < SHARPMIN) { st->sharp = SHARPMIN; }

        /*-------------------------------------------------------*
         * - Find the total excitation.                          *
         * - Find synthesis speech corresponding to exc[].       *
         *-------------------------------------------------------*/

        for (i = 0; i < L_SUBFR;  i++)
          {
            /* exc[i] = gain_pitch*exc[i] + gain_code*code[i]; */
            /* exc[i]  in Q0   gain_pitch in Q14               */
            /* code[i] in Q13  gain_codeode in Q1              */
            
            L_temp = L_mult(st->exc[i+i_subfr], st->gain_pitch);
            L_temp = L_mac(L_temp, code[i], st->gain_code);
            L_temp = L_shl(L_temp, 1);
            st->exc[i+i_subfr] = L_round(L_temp);
          }
        
        Overflow = WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 0);
        if(Overflow != 0)
          {
            /* In case of overflow in the synthesis          */
            /* -> Scale down vector exc[] and redo synthesis */

            for (i = 0; i < PIT_MAX + L_INTERPOL + L_FRAME; i++)
              st->old_exc[i] = shr(st->old_exc[i], 2);

            WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 1);
          }
        else
          WEBRTC_SPL_MEMCPY_W16(st->mem_syn, &synth[i_subfr+L_SUBFR-M], M);

        Az += MP1;              /* interpolated LPC parameters for next subframe */
      }
  }
  
  /*------------*
   *  For G729b
   *-----------*/
  if(bfi == 0) {
    L_temp = 0L;
    for (i = 0; i < L_FRAME; i++) {
      L_temp = L_mac(L_temp, st->exc[i], st->exc[i]);
    } /* may overflow => last level of SID quantizer */
    st->sh_sid_sav = WebRtcSpl_NormW32(L_temp);
    st->sid_sav = L_round(L_shl(L_temp, st->sh_sid_sav));
    st->sh_sid_sav = WebRtcSpl_SubSatW16(16, st->sh_sid_sav);
  }

 /*--------------------------------------------------*
  * Update signal for next frame.                    *
  * -> shift to the left by L_FRAME  exc[]           *
  *--------------------------------------------------*/

  Copy(&st->old_exc[L_FRAME], &st->old_exc[0], PIT_MAX+L_INTERPOL);

  /* for G729b */
  st->past_ftyp = ftyp;

  return;
}