Subversion Repositories FlightCtrl

Rev

Rev 700 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 700 Rev 701
Line 263... Line 263...
263
/*********************************************/
263
/*********************************************/
264
/*  Calculate north direction (heading)      */
264
/*  Calculate north direction (heading)      */
265
/*********************************************/
265
/*********************************************/
266
int16_t MM3_heading(void)
266
int16_t MM3_heading(void)
267
{
267
{
268
        int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
268
        int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
269
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
269
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
270
        int16_t angle;
270
        int16_t angle;
271
        uint16_t div_factor;
271
        uint16_t div_factor;
272
        int16_t heading;
272
        int16_t heading;
Line 303... Line 303...
303
    // tilt compensation
303
    // tilt compensation
Line 304... Line 304...
304
 
304
 
305
        // calibration factor for transforming Gyro Integrals to angular degrees
305
        // calibration factor for transforming Gyro Integrals to angular degrees
Line 306... Line 306...
306
        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
306
        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
307
 
307
 
308
        // calculate sinus cosinus of nick and tilt angle
308
        // calculate sinus cosinus of pitch and tilt angle
309
        angle = (IntegralNick/div_factor);
309
        angle = (IntegralPitch/div_factor);
Line 310... Line 310...
310
        sin_nick = (int32_t)(c_sin_8192(angle));
310
        sin_pitch = (int32_t)(c_sin_8192(angle));
311
        cos_nick = (int32_t)(c_cos_8192(angle));
311
        cos_pitch = (int32_t)(c_cos_8192(angle));
312
 
312
 
Line 313... Line 313...
313
        angle = (IntegralRoll/div_factor);
313
        angle = (IntegralRoll/div_factor);
314
        sin_roll = (int32_t)(c_sin_8192(angle));
314
        sin_roll = (int32_t)(c_sin_8192(angle));
315
        cos_roll = (int32_t)(c_cos_8192(angle));
315
        cos_roll = (int32_t)(c_cos_8192(angle));
Line 316... Line 316...
316
 
316
 
317
        Hx_corr = Hx * cos_nick;
317
        Hx_corr = Hx * cos_pitch;
318
        Hx_corr -= Hz * sin_nick;
318
        Hx_corr -= Hz * sin_pitch;