| 156,7 → 156,7 |
|---|
| ************************************************************************/ |
| |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
| return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis]; |
| } |
| |
| void setStaticAttitudeAngles(void) { |
| 181,9 → 181,6 |
|---|
| // Calibrate hardware. |
| analog_calibrate(); |
| |
| // reset gyro readings |
| // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
| |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| yawAngleDiff = 0; |
| 207,10 → 204,8 |
|---|
| uint8_t axis; |
| |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) |
| / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) |
| / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| differential[axis] = gyroD[axis]; |
| averageAcc[axis] += acc[axis]; |
| } |
| 297,7 → 292,7 |
|---|
| // are less than ....., or reintroduce Kalman. |
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t correction; |
| int32_t temp; |
| if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| 329,11 → 324,10 |
|---|
| 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) (1000L - permilleAcc) * angle[axis] |
| temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| + (int32_t) permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - correction; |
| DebugOut.Analog[16 + axis] = angle[axis] - correction; |
| correctionSum[axis] += angle[axis] - temp; |
| } |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| 368,13 → 362,12 |
|---|
| 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] + 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[16 + axis] = correctionSum[axis]; |
| DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
| DebugOut.Analog[28 + axis] = driftComp[axis]; |
| |