| 156,7 → 156,7 |
|---|
| |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
| return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm; |
| return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
| // return 342L * filteredAcc[axis]; |
| } |
| |
| 332,7 → 332,7 |
|---|
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| 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 = angle[axis]; |
| angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| 340,18 → 340,12 |
|---|
| correctionSum[axis] += angle[axis] - temp; |
| } |
| } else { |
| //debugOut.analog[9] = 0; |
| //debugOut.analog[10] = 0; |
| debugOut.analog[9] = 0; |
| debugOut.analog[10] = 0; |
| // experiment: Kill drift compensation updates when not flying smooth. |
| // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| } |
| |
| int32_t accDerived = getAngleEstimateFromAcc(0); |
| debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| |
| accDerived = getAngleEstimateFromAcc(1); |
| debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| } |
| |
| /************************************************************************ |