fxpt_sine16.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/* -*-C-*-
*******************************************************************************
*
* RCS:          $Id: fxpt_sine16.c,v 1.12 2003/11/22 16:34:28 itsme Exp $
* Description:  16-bit fixed point sine approximation
*
* (C) Copyright 1999, Starium, Ltd., all rights reserved.
*
*******************************************************************************
*/

#include "fxpt.h"
#include "xsin_table.h"

/*
 * -1.0         -0.5            0               +0.5            +1.0
 * 0x8000       0xc000          0x0000          0x4000          0x7fff
 * -pi          -pi/2           0               pi/2            pi
 *
 *      3rd             4th             1st             2nd
 *
 * Table lookup w/ linear interpolation.
 */

#ifdef MY_FLOATING_POINT 

//#include <math.h>

#define FX_FLOAT_TO_Q15(float_const) ((fxpt_16) (((double)float_const) * (FX16_MAX + 1)))
#define FX_FLOAT_TO_Q31(float_const) ((fxpt_32) (((double)float_const) * ((fxpt_64)FX32_MAX + 1)))
#define FX_Q15_TO_FLOAT(fxpt_const) (((double)fxpt_const) / (FX16_MAX + 1))
#define FX_Q31_TO_FLOAT(fxpt_const) (((double)fxpt_const) / ((fxpt_64)FX32_MAX + 1))

fxpt_16 fxpt_sine16 (fxpt_16 x)
{
	return FX_FLOAT_TO_Q15( sin( FX_Q15_TO_FLOAT( x ) * 3.1415926535897931032 ) );
}

fxpt_32 fxpt_sine32 (fxpt_32 x)
{
	return FX_FLOAT_TO_Q31( sin( FX_Q31_TO_FLOAT( x )  * 3.1415926535897931032 ) );
}

fxpt_16 fxpt_cos16 (fxpt_16 x)
{
	return FX_FLOAT_TO_Q15( cos( FX_Q15_TO_FLOAT( x )  * 3.1415926535897931032 ) );
}

fxpt_32 fxpt_cos32 (fxpt_32 x)
{
	return FX_FLOAT_TO_Q31( cos( FX_Q31_TO_FLOAT( x )  * 3.1415926535897931032 ) );
}

#else

fxpt_16
fxpt_sine16 (fxpt_16 in)
{
  fxpt_16 x;
  fxpt_16 q, r;
  fxpt_16 y0, y1, y;

  x = in;
  if (x & 0x4000)               /* 2nd or 4th quadrant?   */
    x = -in;                    /* 2nd -> 3rd, 4th -> 1st */

  x &= 0x7fff;                  /* ignore sign            */

  q = x >> LOG2_STEP;
  r = x & ((1 << LOG2_STEP) - 1);

  y0 = sine_table16[q];
  y1 = sine_table16[q+1];
  y = (((fxpt_32)(y1 - y0) * r) >> LOG2_STEP) + y0;

  return (in < 0) ? -y : y;
}

#if 1

fxpt_32
fxpt_sine32 (fxpt_32 in)
{
  fxpt_32 x;
  fxpt_32 q, r;
  fxpt_32 y0, y1, y;

  x = in;
  if (x & 0x40000000L)           /* 2nd or 4th quadrant?   */
    x = -in;                    /* 2nd -> 3rd, 4th -> 1st */

  x &= 0x7fffffffL;              /* ignore sign            */

  q = x >> (LOG2_STEP+16);
  r = x & ((1 << (LOG2_STEP+16)) - 1);

  y0 = sine_table32[q];
  y1 = sine_table32[q+1];
  y = (((fxpt_64)(y1 - y0) * r) >> (LOG2_STEP+16)) + y0;

  return (in < 0) ? -y : y;
}

#else

fxpt_32
fxpt_sine32 (fxpt_32 x)
{
  return fxpt_deposit_h(fxpt_sine16(fxpt_extract_h(x)));
}

#endif

#endif

sine_incr_t
fxpt_compute_sine_incr (int freq, int sample_rate)
{
  return ((sine_incr_t) (((fxpt_64)64 * 1024 * freq * 64) / sample_rate));
}