|
楼主 |
发表于 2015-3-17 10:01:58
|
显示全部楼层
realAX=ay_gps;
realAY=ax_gps;
realAZ=az_gps;
isMotionless = 1;
isMotionless = isMotionless && (fabs(gx_gps-gx_gpsr)<SELF_TEST_MOTIONLESS_BOUNDARY);
isMotionless = isMotionless && (fabs(gy_gps-gy_gpsr)<SELF_TEST_MOTIONLESS_BOUNDARY);
isMotionless = isMotionless && (fabs(gz_gps-gz_gpsr)<SELF_TEST_MOTIONLESS_BOUNDARY);
IMUAEX = f_n[0]; //将重力单位化为加速度单位
IMUAEY = f_n[1];
IMUAEZ = f_n[2]-10.33;
//屏蔽有害加速度,不详细计算滤波器前的简单滤波器
if (fabs(IMUAEX) < IMUE_ZERO_CORRECTION_BOUNDARY) {
IMUAEX = 0;
}
if (fabs(IMUAEY) < IMUE_ZERO_CORRECTION_BOUNDARY) {
IMUAEY = 0;
}
if (fabs(IMUAEZ) < 0.1) {
IMUAEZ = 0;
}
Kalman_Filter_High(Altitude/100,(-IMUAEZ)); } |
|