| 239,9 → 239,8 |
|---|
| ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
| / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) |
| / ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF |
| * MATH_UNIT_FACTOR)); |
| / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
| * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
| + ((int32_t) yawRate * cosroll) / cospitch; |
| } |
| 327,8 → 326,7 |
|---|
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| DebugOut.Analog[9 + axis] = (10 * accDerived) |
| / GYRO_DEG_FACTOR_PITCHROLL; |
| 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; |
| 370,16 → 368,14 |
|---|
| timer = DRIFTCORRECTION_TIME; |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| // Take the sum of corrections applied, add it to delta |
| deltaCorrection = (correctionSum[axis] |
| * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) |
| / DRIFTCORRECTION_TIME; |
| deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR |
| + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| |
| DebugOut.Analog[18 + axis] = deltaCorrection |
| / staticParams.GyroAccTrim; |
| DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
| DebugOut.Analog[28 + axis] = driftComp[axis]; |
| |
| correctionSum[axis] = 0; |
| 432,16 → 428,16 |
|---|
| v = abs(angle[ROLL] / 512); |
| if (v > w) |
| w = v; |
| correction = w/8 + 1; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (compassHeading < 0) |
| error = 0; // disable yaw drift compensation if compass heading is undefined |
| else |
| if (abs(yawRate) > 128) { // spinning fast |
| else if (abs(yawRate) > 128) { // spinning fast |
| error = 0; |
| } else { |
| // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
| error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
| error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
| % 360) - 180; |
| } |
| if (!ignoreCompassTimer && w < 25) { |
| yawGyroDrift += error; |
| 456,10 → 452,11 |
|---|
| yawGyroHeading += (error * 8) / correction; |
| |
| /* |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| */ |
| w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32; |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| */ |
| w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) |
| / 32; |
| |
| // As readable formula: |
| // w = dynamicParams.CompassYawEffect * (1-w/32); |
| 468,7 → 465,9 |
|---|
| if (!ignoreCompassTimer) { |
| v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| // yawGyroHeading - compassCourse on a -180..179 degree interval. |
| r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180; |
| r |
| = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
| % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * dynamicParams.CompassYawEffect; |