Rev 903 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 903 | Rev 916 | ||
---|---|---|---|
Line 369... | Line 369... | ||
369 | { |
369 | { |
370 | printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max); |
370 | printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max); |
371 | debugcounter = 0; |
371 | debugcounter = 0; |
372 | } |
372 | } |
Line 373... | Line 373... | ||
373 | 373 | ||
374 | // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
374 | // If gas is less than 100, stop calibration with a delay of 0.5 seconds |
375 | if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
375 | if (PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 100) measurement--; |
376 | } |
376 | } |
377 | // Rage of all axis |
377 | // Rage of all axis |
378 | MM3_calib.X_range = (x_max - x_min); |
378 | MM3_calib.X_range = (x_max - x_min); |
379 | MM3_calib.Y_range = (y_max - y_min); |
379 | MM3_calib.Y_range = (y_max - y_min); |
Line 398... | Line 398... | ||
398 | /*********************************************/ |
398 | /*********************************************/ |
399 | /* Calculate north direction (heading) */ |
399 | /* Calculate north direction (heading) */ |
400 | /*********************************************/ |
400 | /*********************************************/ |
401 | void MM3_Heading(void) |
401 | void MM3_Heading(void) |
402 | { |
402 | { |
403 | int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw; |
403 | int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
404 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
404 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
405 | int16_t angle; |
405 | int16_t angle; |
406 | uint16_t div_factor; |
406 | uint16_t div_factor; |
407 | int16_t heading; |
407 | int16_t heading; |
Line 437... | Line 437... | ||
437 | // tilt compensation |
437 | // tilt compensation |
Line 438... | Line 438... | ||
438 | 438 | ||
439 | // calibration factor for transforming Gyro Integrals to angular degrees |
439 | // calibration factor for transforming Gyro Integrals to angular degrees |
Line 440... | Line 440... | ||
440 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
440 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
441 | 441 | ||
442 | // calculate sinus cosinus of pitch and tilt angle |
442 | // calculate sinus cosinus of nick and tilt angle |
443 | angle = (IntegralPitch/div_factor); |
443 | angle = (IntegralNick/div_factor); |
Line 444... | Line 444... | ||
444 | sin_pitch = (int32_t)(c_sin_8192(angle)); |
444 | sin_nick = (int32_t)(c_sin_8192(angle)); |
445 | cos_pitch = (int32_t)(c_cos_8192(angle)); |
445 | cos_nick = (int32_t)(c_cos_8192(angle)); |
446 | 446 | ||
Line 447... | Line 447... | ||
447 | angle = (IntegralRoll/div_factor); |
447 | angle = (IntegralRoll/div_factor); |
448 | sin_roll = (int32_t)(c_sin_8192(angle)); |
448 | sin_roll = (int32_t)(c_sin_8192(angle)); |
449 | cos_roll = (int32_t)(c_cos_8192(angle)); |
449 | cos_roll = (int32_t)(c_cos_8192(angle)); |
Line 450... | Line 450... | ||
450 | 450 | ||
451 | Hx_corr = Hx * cos_pitch; |
451 | Hx_corr = Hx * cos_nick; |
452 | Hx_corr -= Hz * sin_pitch; |
452 | Hx_corr -= Hz * sin_nick; |