| 124,7 → 124,7 |
|---|
| #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
| |
| int32_t correctionSum[2] = {0,0}; |
| int16_t correctionSum[2] = {0,0}; |
| |
| /* |
| * Experiment: Compensating for dynamic-induced gyro biasing. |
| 165,6 → 165,7 |
|---|
| dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
| |
| dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0; |
| correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| |
| // Calibrate hardware. |
| analog_calibrate(); |
| 178,6 → 179,7 |
|---|
| |
| // update compass course to current heading |
| compassCourse = compassHeading; |
| |
| // Inititialize YawGyroIntegral value with current compass heading |
| yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
| |
| 199,6 → 201,7 |
|---|
| rate[axis] = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| differential[axis] = gyroD[axis]; |
| } |
| |
| yawRate = yawGyro + dynamicOffsetYaw; |
| |
| // We are done reading variables from the analog module. |
| 237,10 → 240,6 |
|---|
| ACYawRate = yawRate; |
| } |
| |
| DebugOut.Analog[3] = ACRate[PITCH]; |
| DebugOut.Analog[4] = ACRate[ROLL]; |
| DebugOut.Analog[5] = ACYawRate; |
| |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| 273,7 → 272,7 |
|---|
| /************************************************************************ |
| * A kind of 0'th order integral correction, that corrects the integrals |
| * directly. This is the "gyroAccFactor" stuff in the original code. |
| * There is (there) also what I would call a "minus 1st order correction" |
| * There is (there) also a drift compensation |
| * - it corrects the differential of the integral = the gyro offsets. |
| * That should only be necessary with drifty gyros like ENC-03. |
| ************************************************************************/ |
| 282,13 → 281,13 |
|---|
| // are less than ....., or reintroduce Kalman. |
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| if(!looping && //((ZAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction |
| ZAcc >= -dynamicParams.UserParams[7] && ZAcc <= dynamicParams.UserParams[7]) { |
| int32_t correction; |
| if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] = 1; |
| |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| uint8_t debugFullWeight = 1; |
| int32_t accDerived[2]; |
| int32_t accDerived; |
| |
| if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
| permilleAcc /= 2; |
| 304,11 → 303,23 |
|---|
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| accDerived[axis] = getAngleEstimateFromAcc(axis); |
| correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]); |
| accDerived = getAngleEstimateFromAcc(axis); |
| DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
| |
| correctionSum[axis] += angle[axis] - correction; |
| |
| // There should not be a risk of overflow here, since the integrals do not exceed a few 100000. |
| angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L; |
| // change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
| // = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
| // = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 |
| // = permilleAcc * (accDerived - angle[axis]) / 1000 |
| |
| // Experiment: Do not acutally apply the correction. See if drift compensation does that. |
| // angle[axis] += correction / 1000; |
| } |
| |
| DebugOut.Digital[1] = debugFullWeight; |
| 322,25 → 333,28 |
|---|
| * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
| * cause of it: Gyro drift, vibration and rounding errors. |
| * All the corrections made in correctIntegralsByAcc0thOrder over |
| * MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is |
| * then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx. |
| * DRIFTCORRECTION_TIME cycles are summed up. This number is |
| * then divided by DRIFTCORRECTION_TIME to get the approx. |
| * correction that should have been applied to each iteration to fix |
| * the error. This is then added to the dynamic offsets. |
| ************************************************************************/ |
| // 2 times / sec. |
| #define DRIFTCORRECTION_TIME 488/2 |
| void driftCompensation(void) { |
| // 2 times / sec. = 488/2 |
| #define DRIFTCORRECTION_TIME 256L |
| void driftCorrection(void) { |
| static int16_t timer = DRIFTCORRECTION_TIME; |
| int16_t deltaCompensation; |
| int16_t deltaCorrection; |
| uint8_t axis; |
| if (! --timer) { |
| timer = DRIFTCORRECTION_TIME; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| deltaCompensation = ((correctionSum[axis] + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
| CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp); |
| dynamicOffset[axis] += deltaCompensation / staticParams.GyroAccTrim; |
| // Take the sum of corrections applied, add it to delta |
| deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[28 + axis] = dynamicOffset[axis]; |
| correctionSum[axis] = 0; |
| DebugOut.Analog[28 + axis] = dynamicOffset; |
| } |
| } |
| } |
| 351,9 → 365,18 |
|---|
| void calculateFlightAttitude(void) { |
| getAnalogData(); |
| integrate(); |
| |
| DebugOut.Analog[6] = ACRate[PITCH]; |
| DebugOut.Analog[7] = ACRate[ROLL]; |
| DebugOut.Analog[8] = ACYawRate; |
| |
| DebugOut.Analog[3] = rate_PID[PITCH]; |
| DebugOut.Analog[4] = rate_PID[ROLL]; |
| DebugOut.Analog[5] = yawRate; |
| |
| #ifdef ATTITUDE_USE_ACC_SENSORS |
| correctIntegralsByAcc0thOrder(); |
| driftCompensation(); |
| driftCorrection(); |
| #endif |
| } |
| |