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