| 220,6 → 220,8 |
|---|
| * - it corrects the differential of the integral = the gyro offsets. |
| * That should only be necessary with drifty gyros like ENC-03. |
| ************************************************************************/ |
| #define LOG_DIVIDER 12 |
| #define DIVIDER (1L << LOG_DIVIDER) |
| void correctIntegralsByAcc0thOrder(void) { |
| // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
| // are less than ....., or reintroduce Kalman. |
| 227,55 → 229,45 |
|---|
| uint8_t axis; |
| int32_t temp; |
| |
| uint8_t ca = controlActivity >> 8; |
| uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
| uint16_t ca = controlActivity >> 6; |
| uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
| if (!controlActivityWeighted) controlActivityWeighted = 1; |
| uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
| if (!accVectorWeighted) accVectorWeighted = 1; |
| |
| if (highControlActivity) { |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| } else { |
| debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| } |
| uint8_t accPart = staticParams.zerothOrderCorrection; |
| int32_t accDerived; |
| |
| if (accVector <= dynamicParams.maxAccVector) { |
| debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| debugOut.analog[14] = controlActivity; |
| debugOut.analog[15] = accVector; |
| |
| uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
| int32_t accDerived; |
| /* |
| if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| |
| /* |
| if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| |
| if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| debugOut.analog[20] = controlActivityWeighted; |
| debugOut.analog[21] = accVectorWeighted; |
| debugOut.analog[24] = accVector; |
| |
| if (highControlActivity) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| } |
| } |
| |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| temp = attitude[axis]; |
| attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| + (int32_t) permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += attitude[axis] - temp; |
| } |
| } else { |
| // experiment: Kill drift compensation updates when not flying smooth. |
| // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| accPart /= controlActivityWeighted; |
| accPart /= accVectorWeighted; |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| temp = attitude[axis]; |
| attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER; |
| correctionSum[axis] += attitude[axis] - temp; |
| } |
| } |
| |
| 351,7 → 343,7 |
|---|
| int32_t error; |
| |
| if (commands_isCalibratingCompass()) { |
| debugOut.analog[30] = -1; |
| //debugOut.analog[30] = -1; |
| return; |
| } |
| |
| 362,13 → 354,13 |
|---|
| |
| // Compass is invalid, skip. |
| if (magneticHeading < 0) { |
| debugOut.analog[30] = -2; |
| //debugOut.analog[30] = -2; |
| return; |
| } |
| |
| // Spinning fast, skip |
| if (abs(yawRate) > 128) { |
| debugOut.analog[30] = -3; |
| // debugOut.analog[30] = -3; |
| return; |
| } |
| |
| 375,11 → 367,11 |
|---|
| // Otherwise invalidated, skip |
| if (ignoreCompassTimer) { |
| ignoreCompassTimer--; |
| debugOut.analog[30] = -4; |
| //debugOut.analog[30] = -4; |
| return; |
| } |
| |
| debugOut.analog[30] = magneticHeading; |
| //debugOut.analog[30] = magneticHeading; |
| |
| // TODO: Find computational cost of this. |
| error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
| 395,10 → 387,9 |
|---|
| //debugOut.analog[30] = correction; |
| |
| // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
| // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
| // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
| // could have effect on control elsewhere, like in compassControl.c . |
| // and to the heading error (the angle of yaw that the copter is off the set heading). |
| heading += correction; |
| headingError += correction; |
| intervalWrap(&heading, YAWOVER360); |
| |
| // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |