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