|
|
void updateACC(){
uint8_t axis;
float accMagSum = 0;
for (axis = 0; axis < 3; axis++) {
accLPF[axis] = accADC[axis];
accMagSum += accLPF[axis]*accLPF[axis];
}
// 24 us
accMagSum = accMagSum*100.0/(ACC_1G*ACC_1G);
utilLP_float(&accMag, accMagSum, (1.0f/ACC_LPF_FACTOR));
}
void updateACCAttitude(){
uint8_t axis;
// 80 us
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (( 36 < accMag && accMag < 196 ) || disableAccGtest) {
for (axis = 0; axis < 3; axis++) {
//utilLP_float(&EstG.A[axis], accLPF[axis], AccComplFilterConst);
EstG.A[axis] = EstG.A[axis] * (1.0 - AccComplFilterConst) + accLPF[axis] * AccComplFilterConst;
}
}
}
第二段代码可以理解，对加速度值做一阶低通滤波。但第一段代码完全没弄懂是用来做什么的。。。
这段代码中accMAg是什么， 36 < accMag && accMag < 196又是怎么得来的。 |
阿莫论坛20周年了！感谢大家的支持与爱护！！
曾经有一段真挚的爱情摆在我的面前，我没有珍惜，现在想起来，还好我没有珍惜……
|
发表于 2015-8-4 14:51:16