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