| 155,7 → 155,8 |
|---|
| ************************************************************************/ |
| |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
| int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256; |
| return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
| } |
| |
| void setStaticAttitudeAngles(void) { |
| 295,9 → 296,16 |
|---|
| // 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; |
| |
| uint8_t ca = controlActivity >> 8; |
| uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
| |
| if (highControlActivity) { |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| } else { |
| debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| } |
| |
| if (accVector <= dynamicParams.maxAccVector) { |
| debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| |
| 314,15 → 322,14 |
|---|
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| uint8_t ca = controlActivity >> 8; |
| |
| if (ca > staticParams.maxControlActivity) { // reduce effect during stick control activity |
| if (highControlActivity) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| } |
| } else { |
| debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| } |
| |
| /* |