初级会员
- 积分
- 119
- 金钱
- 119
- 注册时间
- 2016-7-5
- 在线时间
- 24 小时
|
发表于 2016-9-6 13:54:46
|
显示全部楼层
#define Kp 0.8f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.001f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.005f // half the sample period采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
//定义系统的欧拉角
typedef struct
{
float ROLL;
float PITCH;
float YAW;
}ANGLE;
ANGLE Q_ANGLE;
/*******************************************************************************
快速计算 1/Sqrt(x)，源自雷神3的一段代码，神奇的0x5f3759df！比正常的代码快4倍
*******************************************************************************/
float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
void IMUupdate1(float gx, float gy, float gz, float ax, float ay, float az)
{
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;
if(ax*ay*az==0)
return;
norm = invSqrt(ax*ax + ay*ay + az*az); //acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w) 估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2); //四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
OLED_ShowNum(0,50,q0q2,2,16);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
//gz = gz + Kp*ez+ ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
//四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// 正常化四元数
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//四元数转化成欧拉角
//Q_ANGLE.YAW = 0.002 * MPU6050_GYRO_LAST.Z*Gyro_Gain; // yaw GYRO_I.Z; atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3+1)* 57.3;
Q_ANGLE.YAW = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3+1)* 57.3;//yaw
Q_ANGLE.PITCH = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.ROLL = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
；楼主，能看下我的互补拿错了吗？
|
|
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
楼主
