Dear TI Experts,
In the fixed point version of code example for slaa518a app.note:
Nine-Axis Sensor Fusion Using the Direction Cosine
Matrix Algorithm on the MSP430F5xx Family
There is a function called my_sin_cos which calculates sin and cos for a given angle.
Could you please let me know some reference where the calculation algorithm is described.
The code snippet is below:
Regards,
Artak
void my_sin_cos(int16_t A, int16_t *s, int16_t *c)
{
// input = radians in Q4.12
// ouput in Q4.12
int16_t Y,X,i,dx,dy,da,s_sgn, c_sgn, Y1, X1;
//const int16_t LUT[13] = {3217, 1899, 1003, 509, 256, 128, 64, 32, 16, 8, 4, 2, 1}; //Q4.12
// check if A is > pi or <-pi
if (A >= 0x3244) // pi in Q4.12
{
A = A-0x6488; // 2*pi in Q4.12
}
else if (A <= -0x3244)
{
A = A + 0x6488;
}
/*** at this point -pi <= A <= pi ***/
Y=0;
X=0x09b7; // stg:0.60725 in Q4.12
s_sgn=0;
c_sgn=0;
if (A < 0)
{
A=-A;
s_sgn=1;
}
/*** at this point 0 <= A <= pi ***/
if (A > 0x1922 ) // pi/2 in Q4.12
{
A = 0x3244-A; // pi - A
c_sgn=1;
}
/*** at this point 0 <= A <= pi/2 ***/
Y1=Y;
X1=X;
for(i=0;i<8;i++)
{
dx=(X1>>i);
dy=(Y1>>i);
da=_AngLUT[i];
if (A>=0)
{
X=X-dy;
Y=Y+dx;
A=A-da;
}
else
{
X=X+dy;
Y=Y-dx;
A=A+da;
}
Y1=Y;
X1=X;
if (X<0) // logic to take care of neg div in fixed
{
if (-X < 0x0001<<i+1)
{X1=0;}
else
{X1+=(0x0001<<i);}
}
if (Y<0)
{
if (-Y < 0x0001<<i+1)
{Y1=0;}
else
{Y1+=(0x0001<<i);}
}
}
if (s_sgn==1)
Y=-Y;
if (c_sgn==1)
X=-X;
*s=Y; //(Q4.12)
*c=X;
}