/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 |