Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 292 → Rev 293

/trunk/compass.c
134,7 → 134,7
Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
// calculate heading
tmp = (s16)(c_tan2_546(Hy, Hx)/546L);
tmp = (s16)(c_atan2_546(Hy, Hx)/546L);
if (tmp > 0) tmp = 360 - tmp;
else tmp = -tmp;
Compass_Heading = tmp;
/trunk/mymath.c
39,8 → 39,44
return (c_sin_8192(90 - angle));
}
 
// higher resolution angle in deg is arg/div
s16 c_sin_8192_res(s16 arg, s16 div)
{
s16 angle, rest;
s32 tmp;
 
angle = arg/div;
rest = arg%div;
 
if(rest>0)
{
tmp = (div-rest)*(s32)c_sin_8192(angle);
tmp += rest * (s32)c_sin_8192(angle+1);
tmp /= div;
return(tmp);
}
else if(rest<0)
{
tmp = (div+rest)*(s32)c_sin_8192(angle);
tmp -= rest * (s32)c_sin_8192(angle-1);
tmp /= div;
return(tmp);
}
else
{
return(c_sin_8192(angle));
}
 
}
 
s16 c_cos_8192_res(s16 arg, s16 div)
{
return(c_sin_8192_res(90*div - arg, div));
}
 
 
// integer based atan2 that returns angle in counts of 1/546.13°
s32 c_tan2_546(s32 y, s32 x)
s32 c_atan2_546(s32 y, s32 x)
{
s32 qx, qy, q;
/trunk/mymath.h
1,8 → 1,15
#ifndef _MYMATH_H
#define _MYMATH_H
 
// trigonometry resolution of angle is 1 deg
s16 c_sin_8192(s16 angle);
s16 c_cos_8192(s16 angle);
s32 c_tan2_546(s32 y, s32 x);
 
// higher resolution trigonometry with angle in deg is arg/div
s16 c_sin_8192_res(s16 arg, s16 div);
s16 c_cos_8192_res(s16 arg, s16 div);
 
// fast arctan implementation
s32 c_atan2_546(s32 y, s32 x);
 
#endif // _MYMATH_H