Subversion Repositories FlightCtrl

Rev

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

Rev 1805 Rev 1821
Line 237... Line 237...
237
        int16_t tanpitch = int_tan(angle[PITCH]);
237
        int16_t tanpitch = int_tan(angle[PITCH]);
238
#define ANTIOVF 512
238
#define ANTIOVF 512
239
        ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
239
        ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
240
                        * sinroll) / (int32_t) MATH_UNIT_FACTOR;
240
                        * sinroll) / (int32_t) MATH_UNIT_FACTOR;
241
        ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll
241
        ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll
242
                        / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL])
242
                        / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF
243
                        / ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF
243
                        * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR));
244
                        * MATH_UNIT_FACTOR));
-
 
245
        ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch
244
        ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch
246
                        + ((int32_t) yawRate * cosroll) / cospitch;
245
                        + ((int32_t) yawRate * cosroll) / cospitch;
247
}
246
}
Line 248... Line 247...
248
 
247
 
Line 325... Line 324...
325
                /*
324
                /*
326
                 * Add to each sum: The amount by which the angle is changed just below.
325
                 * Add to each sum: The amount by which the angle is changed just below.
327
                 */
326
                 */
328
                for (axis = PITCH; axis <= ROLL; axis++) {
327
                for (axis = PITCH; axis <= ROLL; axis++) {
329
                        accDerived = getAngleEstimateFromAcc(axis);
328
                        accDerived = getAngleEstimateFromAcc(axis);
330
                        DebugOut.Analog[9 + axis] = (10 * accDerived)
329
                        DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
331
                                        / GYRO_DEG_FACTOR_PITCHROLL;
-
 
Line 332... Line 330...
332
 
330
 
333
                        // 1000 * the correction amount that will be added to the gyro angle in next line.
331
                        // 1000 * the correction amount that will be added to the gyro angle in next line.
334
                        correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
332
                        correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
335
                        angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis]
333
                        angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis]
Line 368... Line 366...
368
        uint8_t axis;
366
        uint8_t axis;
369
        if (!--timer) {
367
        if (!--timer) {
370
                timer = DRIFTCORRECTION_TIME;
368
                timer = DRIFTCORRECTION_TIME;
371
                for (axis = PITCH; axis <= ROLL; axis++) {
369
                for (axis = PITCH; axis <= ROLL; axis++) {
372
                        // Take the sum of corrections applied, add it to delta
370
                        // Take the sum of corrections applied, add it to delta
373
                        deltaCorrection = (correctionSum[axis]
371
                        deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR
374
                                        * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2)
372
                                        + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME;
375
                                        / DRIFTCORRECTION_TIME;
-
 
376
                        // Add the delta to the compensation. So positive delta means, gyro should have higher value.
373
                        // Add the delta to the compensation. So positive delta means, gyro should have higher value.
377
                        driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
374
                        driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
378
                        CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
375
                        CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
379
                        // DebugOut.Analog[11 + axis] = correctionSum[axis];
376
                        // DebugOut.Analog[11 + axis] = correctionSum[axis];
Line 380... Line 377...
380
 
377
 
381
                        DebugOut.Analog[18 + axis] = deltaCorrection
-
 
382
                                        / staticParams.GyroAccTrim;
378
                        DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim;
Line 383... Line 379...
383
                        DebugOut.Analog[28 + axis] = driftComp[axis];
379
                        DebugOut.Analog[28 + axis] = driftComp[axis];
384
 
380
 
385
                        correctionSum[axis] = 0;
381
                        correctionSum[axis] = 0;
Line 430... Line 426...
430
                // get maximum attitude angle
426
                // get maximum attitude angle
431
                w = abs(angle[PITCH] / 512);
427
                w = abs(angle[PITCH] / 512);
432
                v = abs(angle[ROLL] / 512);
428
                v = abs(angle[ROLL] / 512);
433
                if (v > w)
429
                if (v > w)
434
                        w = v;
430
                        w = v;
435
                correction = w/8 + 1;
431
                correction = w / 8 + 1;
436
                // calculate the deviation of the yaw gyro heading and the compass heading
432
                // calculate the deviation of the yaw gyro heading and the compass heading
437
                if (compassHeading < 0)
433
                if (compassHeading < 0)
438
                        error = 0; // disable yaw drift compensation if compass heading is undefined
434
                        error = 0; // disable yaw drift compensation if compass heading is undefined
439
                else
-
 
440
                if (abs(yawRate) > 128) { // spinning fast
435
                else if (abs(yawRate) > 128) { // spinning fast
441
                        error = 0;
436
                        error = 0;
442
                } else {
437
                } else {
443
                        // compassHeading - yawGyroHeading, on a -180..179 deg interval.
438
                        // compassHeading - yawGyroHeading, on a -180..179 deg interval.
444
                        error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
439
                        error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW))
-
 
440
                                        % 360) - 180;
445
                }
441
                }
446
                if (!ignoreCompassTimer && w < 25) {
442
                if (!ignoreCompassTimer && w < 25) {
447
                        yawGyroDrift += error;
443
                        yawGyroDrift += error;
448
                        // Basically this gets set if we are in "fix" mode, and when starting.
444
                        // Basically this gets set if we are in "fix" mode, and when starting.
449
                        if (updateCompassCourse) {
445
                        if (updateCompassCourse) {
Line 454... Line 450...
454
                        }
450
                        }
455
                }
451
                }
456
                yawGyroHeading += (error * 8) / correction;
452
                yawGyroHeading += (error * 8) / correction;
Line 457... Line 453...
457
 
453
 
458
                /*
454
                /*
459
                w = (w * dynamicParams.CompassYawEffect) / 32;
455
                 w = (w * dynamicParams.CompassYawEffect) / 32;
460
                w = dynamicParams.CompassYawEffect - w;
456
                 w = dynamicParams.CompassYawEffect - w;
461
                */
457
                 */
-
 
458
                w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect)
Line 462... Line 459...
462
                w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32;
459
                                / 32;
463
 
460
 
Line 464... Line 461...
464
                // As readable formula:
461
                // As readable formula:
465
                // w = dynamicParams.CompassYawEffect * (1-w/32);
462
                // w = dynamicParams.CompassYawEffect * (1-w/32);
466
 
463
 
467
                if (w >= 0) { // maxAttitudeAngle < 32
464
                if (w >= 0) { // maxAttitudeAngle < 32
-
 
465
                        if (!ignoreCompassTimer) {
468
                        if (!ignoreCompassTimer) {
466
                                v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
-
 
467
                                // yawGyroHeading - compassCourse on a -180..179 degree interval.
469
                                v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
468
                                r
470
                                // yawGyroHeading - compassCourse on a -180..179 degree interval.
469
                                                = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
471
                                r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180;
470
                                                                % 360) - 180;
472
                                v = (r * w) / v; // align to compass course
471
                                v = (r * w) / v; // align to compass course
473
                                // limit yaw rate
472
                                // limit yaw rate