📄 tfr16.c
字号:
/* File: tfr16.c */
/*--------------------------------------------------------------
*
* (C) 2000 (c) Freescale Semiconductor
*-------------------------------------------------------------*/
/*--------------------------------------------------------------
* Revision History:
*
* VERSION CREATED BY MODIFIED BY DATE COMMENTS
* ------- ---------- ----------- ----- --------
* 1.0 Meera S. P. - 17-02-2000 Reviewed.
*
*-------------------------------------------------------------*/
/*---------------------------------------------------------------
* 16 bit Fractional Trignometric Functions
*----------------------------------------------------------------*/
#include "port.h"
#include "tfr16.h"
#include "stdio.h"
#include "MFR1.h"
#include "prototype.h"
#include "mem.h"
#define CFF(x) (Frac16)(x * 32767.0)
#define SWG_90_DEGREES 8192
#define SWG_180_DEGREES 16384
#define SWG_270_DEGREES 24576
#define SWG_360_DEGREES 32767
#define SWG_SIGN_POSITIVE 1
#define SWG_SIGN_NEGATIVE -1
#define SWG_ONEHALF 16384
#define PI_PLUS 32768
#define PI_MINUS -32767
#define SWG_NEG_MAX -32768
#define PI_HALF_PLUS 16384
#define PI_HALF_MINUS -16384
void tfr16SineWaveGenIDTLC (tfr16_tSineWaveGenIDTL *, Frac16 *, UInt16);
void tfr16SineWaveGenRDTLC (tfr16_tSineWaveGenRDTL *, Frac16 *, UInt16);
void tfr16SineWaveGenRDITLC (tfr16_tSineWaveGenRDITL *, Frac16 *, UInt16);
void tfr16SineWaveGenRDITLQC(tfr16_tSineWaveGenRDITLQ *, Frac16 *, UInt16);
void tfr16SineWaveGenPAMC (tfr16_tSineWaveGenPAM *, Frac16 *, UInt16);
void tfr16SineWaveGenDOMC (tfr16_tSineWaveGenDOM *, Frac16 *, UInt16);
Frac16 tfr16WaveGenRDITLQC (tfr16_tWaveGenRDITLQ *, Frac16);
Frac16 tfr16SinPIxLUTC (tfr16_tSinPIxLUT *, Frac16);
void tfr16CosPIxLUTInitC (tfr16_tCosPIxLUT * pSWG,
Frac16 * pSineTable,
UInt16 SineTableLength);
Frac16 tfr16CosPIxLUTC (tfr16_tCosPIxLUT * pSWG, Frac16);
/***************************************************************************************
* The Scale Factor is calculated using matlab. *
* *
* ex : *
* For Sine Coefs. *
* c = [pi -pi^3/3! pi^5/5! -pi^7/7! pi^9/9! -pi^11/11! pi^13/13! -pi^15/15! pi^17/17!] *
* cmax = max(abs(c)) *
* m = ceil(log2(cmax)) *
* SCALE_FACT = 2^m *
* *
* SineCoefs(I) = c(I)/8 *
***************************************************************************************/
#define SCALE_FACT 8
const Frac16 SineCoefs[] = {
CFF(0.39269908169872),
CFF(-0.64596409750625),
CFF(0.31877050498467),
CFF(-0.07446482317004),
CFF(0.01026823582639),
CFF(-0.00092130386821),
CFF(0.00005828785072)
};
const Frac16 AsineCoefs[] = {
CFF(0.31830988618379),
CFF(0.05305164769730),
CFF(0.02387324146378),
CFF(0.01421026277606),
CFF(0.00967087327815),
CFF(0.00712127941391),
CFF(0.00552355646848),
CFF(0.00444514782464)
};
const Frac16 Inv_Threshold = CFF(0.70710678118655);
/*******************************************************
* Trigonometric Functions for 16-bit Fractional
*******************************************************/
/********************************************************************************
* File Name : tfr16.c *
* *
* Function : Frac16 tfr16SinPIx (Frac16 x) *
* *
* Description: *
* This routine computes sine(pi*x) *
* *
* Inputs : *
* 1) x - input data value *
* *
* Outputs : *
* 1) z - sine(pi*x) *
* *
* Algorithm : *
* if(x > 0.5) *
* sin(pi*x) = sin(pi*(1-x)) *
* *
* *
* z = x(SineCoefs(0)+x^2(SineCoefs(1)+x^2(SineCoefs(2)+x^2(SineCoefs(3)+ *
* x^2(SineCoefs(4)+x^2(SineCoefs(5)+SineCoefs(6)x^2)))))) *
********************************************************************************/
Frac16 tfr16SinPIx (Frac16 x)
{
Frac16 z, temp16, sign_flag = 0, i;
Frac32 Acc = 0,temp32;
word Tmp;
asm(move.w Y1,Tmp);
if(x < 0)
{
sign_flag = 1;
x = negate(x);
}
if(x > CFF(0.5))
{
/* sin(pi*x) = sin(pi*(1-x)) */
x = sub(CFF(1),x);
}
z = mult_r(x,x); /* z = x^2 */
Acc = L_deposit_h(SineCoefs[5]);
temp16 = mac_r(Acc,z,SineCoefs[6]); /* temp16 = c5 +c6*x^2 */
/* temp16 = (c0+z(c1+z(c2+z(c3+z(c4+z(c5+c6z)))))) */
for(i = 4; i >= 0; i--)
{
Acc = L_deposit_h(SineCoefs[i]);
temp16 = mac_r(Acc,temp16,z);
}
temp32 = L_mult(temp16,x); /* z = x * temp16 */
/* z = z*8 (as the coefs are scaled down by 8,
the o/p is scaled up by 8) */
temp32 = L_shl(temp32,(Frac16)3);
z = round(temp32);
if (sign_flag == 1)
{
z = negate(z);
}
asm(move.w Tmp,Y1);
return z;
}
/********************************************************************************
* File Name : tfr16.c *
* *
* Function : Frac16 tfr16CosPIx (Frac16 x) *
* *
* Description: *
* This routine computes cosine(pi*x) *
* *
* Inputs : *
* 1) x - input data value *
* *
* Outputs : *
* 1) z - cosine(pi*x) *
* *
* Algorithm : *
* if(x > 0.5) *
* cos(pi*x) = -cos(pi*(1-x)) *
* *
* cos(pi*x) = sin(pi*(0.5-x)) *
* *
********************************************************************************/
Frac16 tfr16CosPIx (Frac16 x)
{
Frac16 z, sign_flag = 0;
// Frac16 temp16, i, temp1, temp 2;
Frac32 Acc = 0;
// Frac32 temp32;
if(x < 0)
{
x = negate(x); //cos(x) = cos(-x)
}
if(x > CFF(0.5))
{
x = sub(CFF(1),x);
sign_flag = 1;
}
x = sub(CFF(0.5),x);
if(sign_flag ==1)
x = add(x,1); /* To gain the precision */
z = tfr16SinPIx(x);
if(sign_flag == 1)
{
z = negate(z);
}
return z;
}
/********************************************************************************
* File Name : tfr16.c *
* *
* Function : Frac16 tfr16ASinOverPI (Frac16 x) *
* *
* Description: *
* This routine computes the Arcsin function of fractional input value x, and *
* divides that result by pi i.e. Arcsin(x)/pi. *
* *
* Inputs : *
* 1) x - input data value *
* *
* Outputs : *
* 1) z - Arcsin(x)/pi *
* *
* Algorithm: *
* if(x >= 0.70710678118655) *
* z = 0.5 - ASinOverPI(sqrt(1-x^2)) *
* else *
* z = ASinOverPI(x) *
* *
* Where : *
* ASinOverPI(x) = x(AsineCoefs(0)+x^2(AsineCoefs(1)+x^2(AsineCoefs(2)+ *
* x^2(AsineCoefs(3)+x^2(AsineCoefs(4)+x^2(AsineCoefs(5)+ *
* x^2(AsineCoefs(6)+AsineCoefs(7)x^2))))))) *
* *
********************************************************************************/
Frac16 tfr16AsinOverPI (Frac16 x)
{
Frac16 z, temp16, sign_flag = 0, i, thres_flag = 0;
Frac32 Acc = 0;
if (x < 0)
{
sign_flag = 1;
x = negate(x);
}
z = mult_r(x,x);
if (x > Inv_Threshold)
{
thres_flag = 1;
x = mfr16Sqrt(sub(CFF(1),add(z,1)));
z = mult_r(x,x);
}
temp16 = AsineCoefs[7];
for(i = 6; i >= 0; i--)
{
Acc = L_deposit_h(AsineCoefs[i]);
temp16 = mac_r(Acc,temp16,z);
}
z = mult_r(temp16,x);
if (thres_flag == 1)
{
z = sub(CFF(0.5),z);
}
if (sign_flag == 1)
{
z = negate(z);
}
return z;
}
/********************************************************************************
* File Name : tfr16.c *
* *
* Function : Frac16 tfr16ACosOverPI (Frac16 x) *
* *
* Description: *
* This routine computes the Arccos function of fractional input value x, and *
* divides that result by pi i.e. Arccos(x)/pi. *
* *
* Inputs : *
* 1) x - input data value *
* *
* Outputs : *
* 1) z - Arccos(x)/pi *
* *
* Algorithm: *
* AtanOverPI(x) = 0.5 - AsinOverPI(x) *
********************************************************************************/
Frac16 tfr16AcosOverPI (Frac16 x)
{
Frac16 z, sign_flag = 0;
if(x < 0)
{
sign_flag = 1;
x = negate(x);
}
z = tfr16AsinOverPI (x);
if (sign_flag == 1)
{
z = negate(z);
}
z = sub(CFF(0.50000000000),z); /*acos(x) = 0.5 - AsinOverPI(x) */
return z;
}
/********************************************************************************
* File Name : tfr16.c *
* *
* Function : Frac16 tfr16AtanOverPI (Frac16 x) *
* *
* Description: *
* This routine computes the Arctan function of fractional input value x, and *
* divides that result by pi i.e. Arctan(x)/pi. *
* *
* Inputs : *
* 1) x - input data value *
* *
* Outputs : *
* 1) z - Arctan(x)/pi *
* *
* Algorithm: *
* AtanOverPI(x) = 0.5 - AsinOverPI(sqrt(1-x^2)/sqrt(1-x^4)) *
********************************************************************************/
Frac16 tfr16AtanOverPI (Frac16 x)
{
Frac16 z, z1, z2, sign_flag = 0;
if(x < 0)
{
sign_flag = 1;
x = negate(x);
}
if ( x == CFF(1))
{
z = CFF(0.25000000000000);
}
else
{
z1 = mult_r(x,x); /* z1 = x^2 */
if (z1 > 0)
z1 = add(z1,1); /* To compensate the precision lost while calculating square */
z2 = mult_r(z1,z1); /* z2 = x^4 */
if (z2 > 0)
z2 = add(z2,1); /* To compensate the precision lost while calculating square */
z1 = mfr16Sqrt(sub(CFF(1),z1)); /* z1 = sqrt(1-x^2) */
z2 = mfr16Sqrt(sub(CFF(1),z2)); /* z2 = sqrt(1-x^4) */
x = div_s(z1,z2); /* x = (sqrt(1-x^2)/sqrt(1-x^4)) */
/* AtanOverPI(x) = 0.5 - AsinOverPI(sqrt(1-x^2)/sqrt(1-x^4)) */
z = sub(CFF(0.5),tfr16AsinOverPI(x));
}
if (sign_flag == 1)
{
z = negate(z);
}
return z;
}
/********************************************************************************
* File Name : tfr16.c *
* *
* Function : Frac16 tfr16Atan2OverPI (Frac16 y,Frac16 x) *
* *
* Description: *
* This routine computes the Arctan(y/x), and divides that result by pi i.e. *
* Arctan(y/x)/pi. *
* *
* Inputs : *
* 1) y - first input data value *
* 2) x - second input data value *
* *
* Outputs : *
* 1) z - Arctan(y/x)/pi *
* *
* Algorithm : *
* if(y/x < 1) *
* z = AtanOverPI(y/x); *
* else *
* z = (0.5 - AtanOverPI(x/y)) *
*********************************************************************************/
Frac16 tfr16Atan2OverPI (Frac16 y, Frac16 x)
{
Frac16 temp16, div_flag = 0, z, sign_flag = 0;
Frac16 ax, ay; /* for CW bug workaround */
if(y == 0 && x == 0)
{
temp16 = 0; /* temp = y/x */
}
else
{
ax = abs_s(x);
ay = abs_s(y);
/* LS010419 - try to keep positive to satisfy ITU (basic_op) constraints */
if ((x<0 && y>=0) || (x>=0 && y<0))
{
sign_flag = 1;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -