/branches/dongfang_FC_rewrite/attitude.c |
---|
155,7 → 155,7 |
************************************************************************/ |
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 512; |
int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
} |