Rev 700 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 700 | Rev 701 | ||
|---|---|---|---|
| Line 263... | Line 263... | ||
| 263 | /*********************************************/ |
263 | /*********************************************/ |
| 264 | /* Calculate north direction (heading) */ |
264 | /* Calculate north direction (heading) */ |
| 265 | /*********************************************/ |
265 | /*********************************************/ |
| 266 | int16_t MM3_heading(void) |
266 | int16_t MM3_heading(void) |
| 267 | { |
267 | { |
| 268 | int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
268 | int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw; |
| 269 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
269 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
| 270 | int16_t angle; |
270 | int16_t angle; |
| 271 | uint16_t div_factor; |
271 | uint16_t div_factor; |
| 272 | int16_t heading; |
272 | int16_t heading; |
| Line 303... | Line 303... | ||
| 303 | // tilt compensation |
303 | // tilt compensation |
| Line 304... | Line 304... | ||
| 304 | 304 | ||
| 305 | // calibration factor for transforming Gyro Integrals to angular degrees |
305 | // calibration factor for transforming Gyro Integrals to angular degrees |
| Line 306... | Line 306... | ||
| 306 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
306 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
| 307 | 307 | ||
| 308 | // calculate sinus cosinus of nick and tilt angle |
308 | // calculate sinus cosinus of pitch and tilt angle |
| 309 | angle = (IntegralNick/div_factor); |
309 | angle = (IntegralPitch/div_factor); |
| Line 310... | Line 310... | ||
| 310 | sin_nick = (int32_t)(c_sin_8192(angle)); |
310 | sin_pitch = (int32_t)(c_sin_8192(angle)); |
| 311 | cos_nick = (int32_t)(c_cos_8192(angle)); |
311 | cos_pitch = (int32_t)(c_cos_8192(angle)); |
| 312 | 312 | ||
| Line 313... | Line 313... | ||
| 313 | angle = (IntegralRoll/div_factor); |
313 | angle = (IntegralRoll/div_factor); |
| 314 | sin_roll = (int32_t)(c_sin_8192(angle)); |
314 | sin_roll = (int32_t)(c_sin_8192(angle)); |
| 315 | cos_roll = (int32_t)(c_cos_8192(angle)); |
315 | cos_roll = (int32_t)(c_cos_8192(angle)); |
| Line 316... | Line 316... | ||
| 316 | 316 | ||
| 317 | Hx_corr = Hx * cos_nick; |
317 | Hx_corr = Hx * cos_pitch; |
| 318 | Hx_corr -= Hz * sin_nick; |
318 | Hx_corr -= Hz * sin_pitch; |