Subversion Repositories FlightCtrl

Rev

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;