Rev 292 | Rev 308 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 292 | Rev 293 | ||
---|---|---|---|
Line 132... | Line 132... | ||
132 | cosroll = (s32)c_cos_8192(tmp); |
132 | cosroll = (s32)c_cos_8192(tmp); |
133 | // tbd. compensation signs and oriantation has to be fixed |
133 | // tbd. compensation signs and oriantation has to be fixed |
134 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
134 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
135 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
135 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
136 | // calculate heading |
136 | // calculate heading |
137 | tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
137 | tmp = (s16)(c_atan2_546(Hy, Hx)/546L); |
138 | if (tmp > 0) tmp = 360 - tmp; |
138 | if (tmp > 0) tmp = 360 - tmp; |
139 | else tmp = -tmp; |
139 | else tmp = -tmp; |
140 | Compass_Heading = tmp; |
140 | Compass_Heading = tmp; |
141 | } |
141 | } |
142 | } |
142 | } |