fs_lib.c

C50_melp.tar.gz为美军2400bps语音压缩编码算法

/*

*2.4 kbps MELP Proposed Federal Standard speech coder
*
*TMS320C5x assembly code
*
*version 1.0
*
*Copyright (c) 1998, Texas Instruments, Inc.  
*
*Texas Instruments has intellectual property rights on the MELP
*algorithm.  The Texas Instruments contact for licensing issues for
*commercial and non-government use is William Gordon, Director,
*Government Contracts, Texas Instruments Incorporated, Semiconductor
*Group (phone [phone]).
*
*/

/*************************************************************************
*
* The following code was hand optimized for the Texas Instuments
* TMS320C5x DSP by DSPCon, Inc.  For information, please contact DSPCon
* at:
* 
*                       DSPCon, Inc.
*                       380 Foothill Road
*                       Bridgewater, New Jersey 08807
*                       [phone]
*                       [e-mail]
*                       www.dspcon.com
*
*************************************************************************/



/*

  fs_lib.c: Fourier series subroutines

*/

/*  compiler include files  */
#include <stdlib.h>
#include <math.h>
#include "spbstd.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "mat.h"
#include "math_lib.h"
#include "fs.h"

#define FAST_FFT

/*  compiler constants */

/*                                                              */
/*      Subroutine FIND_HARM: find Fourier coefficients using   */
/*      FFT of input signal divided into pitch dependent bins.  */
/*                                                              */
/*  Q values:
    input - Q0
    fsmag - Q13
    pitch - Q7 */

#define FFTLENGTH       512
#define MONE_Q15        (Shortword)(-((Longword)1<<15))
#define X05_Q7          (0.5*(1<<7))
#define ONE_Q13         (1<<13)
#define ONE_Q15         (Shortword)(((Longword)1<<15)-1)

/* Memory definition            */
static Longword mag[FFTLENGTH];
extern Shortword fft_data[2*FFTLENGTH];
#define find_hbuf fft_data

void find_harm(Shortword input[],Shortword fsmag[],Shortword pitch,
	       Shortword num_harm,Shortword length)
{
    Shortword i, j, k, iwidth, i2;
    Shortword fwidth;
    Shortword num_harm_Q11;
    Longword  avg;
    Shortword temp;
    Shortword shift, max;

    Longword    L_fsmag[20];

    shift = find_harm_s1(input, length, fsmag, num_harm);


    /* Perform peak-picking on FFT of input signal */

    /* Calculate FFT of complex signal in scratch buffer */
    cfft (input, length, shift);


    /* Implement pitch dependent staircase function             */
    /* Harmonic bin width */
    /* fwidth=Q6 */
    fwidth = shr(divide_s((Shortword)FFTLENGTH,pitch),2);
    /* iwidth = (int) fwidth */
    iwidth = shr(fwidth,6);
        if (iwidth < 2) {
      iwidth = 2;      
    }
    i2 = shr(iwidth,1);

    /* if (num_harm > 0.25*pitch) num_harm = 0.25*pitch */
    /* temp = 0.25*pitch in Q0 */
    temp = shr(pitch,9);

    if (num_harm > temp) {
      num_harm = temp;
    }

    /* initialize avg to make sure that it is non-zero */
    avg = 1;       
    for (k = 0; k < num_harm; k++) {
      /* i = ((k+1)*fwidth) - i2 + 0.5 */ /* Start at peak-i2 */
      temp = extract_l(L_mult(add(k,1),fwidth));  /* temp=Q7 */
      i = shr(add(sub(temp,shl(i2,7)),(Shortword)X05_Q7),7);

      /* Calculate magnitude squared of coefficients            */
      for (j = i; j < i+iwidth; j++) {
	mag[j] = L_add(L_mult(find_hbuf[2*j],find_hbuf[2*j]),
		       L_mult(find_hbuf[(2*j)+1],find_hbuf[(2*j)+1]));
              }

      j = add(i,findmax(&mag[i],iwidth));
      L_fsmag[k] = mag[j];       
      avg = L_add(avg,mag[j]);
    }

    /* Normalize Fourier series values to average magnitude */
    num_harm_Q11 = shl(num_harm,11);
    for (i = 0; i < num_harm; i++) {
      /* temp = num_harm/(avg+ .0001) */
      /* fsmag[i] = sqrt(temp*fsmag[i]) */
      temp = L_divider2(L_fsmag[i],avg,0,0);     /* temp=Q15 */
      temp = mult(num_harm_Q11,temp);    /* temp=Q11 */
      fsmag[i] = shl(sqrt_fxp(temp,11),2);      
    }

} /* find_harm */




/*                                                              */
/*      Subroutine FINDMAX: find maximum value in an            */
/*      input array.                                            */
/*                                                              */
Shortword findmax(Longword input[],Shortword npts)
{
  Shortword register    i, maxloc;
  Longword register  maxval, *p_in;

  p_in = &input[0];
  maxloc = 0;    
  maxval = input[maxloc];    
  for (i = 1; i < npts; i++ ) {
    
    if (*(++p_in) > maxval) {
      maxloc = i;    
      maxval = *p_in;    
    }
  }
  return(maxloc);
}



/* Initialization of wr_array and wi_array */
void fs_init()
{
}