| 265,7 → 265,7 |
|---|
| /*********************************************/ |
| int16_t MM3_heading(void) |
| { |
| int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
| int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw; |
| int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
| int16_t angle; |
| uint16_t div_factor; |
| 305,17 → 305,17 |
|---|
| // calibration factor for transforming Gyro Integrals to angular degrees |
| div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
| |
| // calculate sinus cosinus of nick and tilt angle |
| angle = (IntegralNick/div_factor); |
| sin_nick = (int32_t)(c_sin_8192(angle)); |
| cos_nick = (int32_t)(c_cos_8192(angle)); |
| // calculate sinus cosinus of pitch and tilt angle |
| angle = (IntegralPitch/div_factor); |
| sin_pitch = (int32_t)(c_sin_8192(angle)); |
| cos_pitch = (int32_t)(c_cos_8192(angle)); |
| |
| angle = (IntegralRoll/div_factor); |
| sin_roll = (int32_t)(c_sin_8192(angle)); |
| cos_roll = (int32_t)(c_cos_8192(angle)); |
| |
| Hx_corr = Hx * cos_nick; |
| Hx_corr -= Hz * sin_nick; |
| Hx_corr = Hx * cos_pitch; |
| Hx_corr -= Hz * sin_pitch; |
| Hx_corr /= 8192; |
| |
| Hy_corr = Hy * cos_roll; |