| 295,29 → 295,30 |
|---|
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t temp; |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| |
| if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| uint8_t debugFullWeight = 1; |
| int32_t accDerived; |
| |
| /* |
| 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 |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| if (controlActivity > 10000) { // reduce effect during stick commands |
| permilleAcc /= 4; |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| if (controlActivity > 20000) { // reduce effect during stick commands |
| permilleAcc /= 4; |
| DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| } |
| } |
| |
| if (debugFullWeight) |
| DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| else |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| 332,8 → 333,6 |
|---|
| correctionSum[axis] += angle[axis] - temp; |
| } |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Analog[9] = 0; |
| DebugOut.Analog[10] = 0; |
| |
| 452,7 → 451,8 |
|---|
| |
| if (w >= 0) { // maxAttitudeAngle < 32 |
| if (!ignoreCompassTimer) { |
| v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
| v = 64 + controlActivity / 100; |
| // yawGyroHeading - compassCourse on a -180..179 degree interval. |
| r |
| = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |