| 371,8 → 371,8 |
|---|
| debugcounter = 0; |
| } |
| |
| // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
| if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
| // If gas is less than 100, stop calibration with a delay of 0.5 seconds |
| if (PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 100) measurement--; |
| } |
| // Rage of all axis |
| MM3_calib.X_range = (x_max - x_min); |
| 400,7 → 400,7 |
|---|
| /*********************************************/ |
| void MM3_Heading(void) |
| { |
| int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw; |
| int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
| int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
| int16_t angle; |
| uint16_t div_factor; |
| 439,17 → 439,17 |
|---|
| // calibration factor for transforming Gyro Integrals to angular degrees |
| div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
| |
| // 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)); |
| // 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)); |
| |
| angle = (IntegralRoll/div_factor); |
| sin_roll = (int32_t)(c_sin_8192(angle)); |
| cos_roll = (int32_t)(c_cos_8192(angle)); |
| |
| Hx_corr = Hx * cos_pitch; |
| Hx_corr -= Hz * sin_pitch; |
| Hx_corr = Hx * cos_nick; |
| Hx_corr -= Hz * sin_nick; |
| Hx_corr /= 8192; |
| |
| Hy_corr = Hy * cos_roll; |