无论陀螺仪还是加速度,电子罗盘都校准过,但是最后YAW偏航角还是会飘,慢的几秒一度,快的一秒一度..
检查过好多次,没发现哪里有问题..
- //IMU四元素
- void Calc_Quaternions(_Triaxial_f acc, _Triaxial_f gyro, _Triaxial_f mag)
- {
- float ax = acc.x,ay = acc.y,az = acc.z;
- float gx = gyro.x,gy = gyro.y,gz = gyro.z;
- float mx = mag.x,my = mag.y,mz = mag.z;
- // float mx = 0,my = 0,mz = 0;
- float norm;
- float hx, hy, hz, bx, bz;
- float vx, vy, vz, wx, wy, wz;
- float ex, ey, ez;
- float q0q0 = q0*q0;
- float q0q1 = q0*q1;
- float q0q2 = q0*q2;
- float q0q3 = q0*q3;
- float q1q1 = q1*q1;
- float q1q2 = q1*q2;
- float q1q3 = q1*q3;
- float q2q2 = q2*q2;
- float q2q3 = q2*q3;
- float q3q3 = q3*q3;
-
- float deltaT;
- if(ax*ay*az==0)
- return;
- deltaT = getDeltaT(GetSysTime_us());
-
- //ÖØÁ¦¼ÓËٶȹéÒ»»¯
- norm = invSqrt(ax*ax + ay*ay + az*az);
- ax = ax * norm;
- ay = ay * norm;
- az = az * norm;
- //´ÅÁ¦¼Æ¹éÒ»»¯
- norm = invSqrt(mx*mx* + my*my + mz*mz);
- mx = mx * norm;
- my = my * norm;
- mz = mz * norm;
- hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
- hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
- hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);
-
- bx = sqrt((hx*hx) + (hy*hy));
- bz = hz;
-
- //ÌáÈ¡ËÄÔªÊýµÄµÈЧÓàÏÒ¾ØÕóÖеÄÖØÁ¦·ÖÁ¿
- vx = 2*(q1q3 - q0q2);
- vy = 2*(q0q1 + q2q3);
- vz = q0q0 - q1q1 - q2q2 + q3q3 ;
- wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
- wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
- wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
-
- //ÏòÁ¿²æ»ýµÃ³ö×Ë̬Îó²î
- ex = (ay*vz - az*vy) + (my*wz - mz*wy);
- ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
- ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
- if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
- {
- exInt = exInt + ex * Ki * halfT*deltaT;
- eyInt = eyInt + ey * Ki * halfT*deltaT;
- ezInt = ezInt + ez * Ki * halfT*deltaT;
-
- //»¥²¹Â˲¨£¬×Ë̬Îó²î²¹³¥µ½½ÇËÙ¶ÈÉÏ£¬ÐÞÕý½ÇËٶȻý·ÖƯÒÆ
- gx = gx + Kp*ex + exInt;
- gy = gy + Kp*ey + eyInt;
- gz = gz + Kp*ez + ezInt;
- }
- //Ò»½×Áú¸ñ¿âËþ·¨¸üÐÂËÄÔªÊý
- q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT*deltaT;
- q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT*deltaT;
- q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT*deltaT;
- q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT*deltaT;
- //ËÄÔªÊý¹éÒ»»¯
- norm = invSqrt(q0q0 + q1q1 + q2q2 + q3q3);
- q0 = q0 * norm;
- q1 = q1 * norm;
- q2 = q2 * norm;
- q3 = q3 * norm;
- //ËÄÔªÊýתŷÀ½Ç
- Angle.x = atan2f(2.0f * q2q3 + 2.0f * q0q1, -2.0f * q1q1 - 2.0f * q2q2 + 1) * RAD_TO_DEG;// ·¹ö
- Angle.y = -safe_asin(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG; // ¸©Ñö
- Angle.z = -atan2f(2.0f*q1q2 + 2.0f*q0q3, -2.0f *q2q2 - 2.0f*q3q3 + 1) * RAD_TO_DEG;//Æ«º½
- }
| 
楼主
