|
|
发表于 2014-4-15 10:48:05
|
显示全部楼层
我连这还分不清吗？？？我定义的x轴是rol
/*******************************************************************************
* Function Name : init_quaternion
* Description : 算出初始化四元数q0 q1 q2 q3.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void init_quaternion(void)
{
u8 i;
tg_HMC5883L_TYPE hmc5883l;
signed short int accel[3];
float init_Yaw, init_Pitch, init_Roll;
if(!IIC_Read(MPU6050_Addr, Accel_Xout_H, 6, data_write))
{
accel[0]=((( signed short int)data_write[0])<<8) | data_write[1];
accel[1]=((( signed short int)data_write[2])<<8) | data_write[3];
accel[2]=((((signed short int)data_write[4])<<8) | data_write[5]) + Accel_Zout_Offset;
init_ax=((float)accel[0]) / Accel_4_Scale_Factor; //单位转化成重力加速度的单位：g
init_ay=((float)accel[1]) / Accel_4_Scale_Factor;
init_az=((float)accel[2]) / Accel_4_Scale_Factor;
for(i=0;i<5;i++) //第一次读取的compsaa数据是错的，因此要多读几次保证以后数据正确，芯片bug
{
HMC5883L_Init();
delay_ms(10);
HMC5883L_MultRead(&hmc5883l); //读磁阻仪数据(速度:慢, ms级延时过程)
//进行x y轴的校准，未对z轴进行校准，参考MEMSense的校准方法
init_mx =hmc5883l.hx;
init_my =hmc5883l.hy;
init_mz =hmc5883l.hz;
}
//陀螺仪x轴为前进方向
init_Roll = atan2(init_ay, init_az);
init_Pitch = -asin(init_ax); //init_Pitch = asin(ax / 1);
init_Yaw = -atan2(init_mx*cos(init_Roll) + init_my*sin(init_Roll)*sin(init_Pitch) + init_mz*sin(init_Roll)*cos(init_Pitch),
init_my*cos(init_Pitch) - init_mz*sin(init_Pitch)); //atan2(mx, my);
q0 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw); //w
q1 = sin(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) - cos(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw); //x 绕x轴旋转是roll
q2 = cos(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw); //y 绕y轴旋转是pitch
q3 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw) - sin(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw); //z 绕z轴旋转是Yaw
init_Roll = init_Roll * 57.295780; //弧度转角度
init_Pitch = init_Pitch * 57.295780;
init_Yaw = init_Yaw * 57.295780;
if(init_Yaw < 0) //将Yaw的范围转成0-360
{
init_Yaw = init_Yaw + 360;
}
if(init_Yaw > 360)
{
init_Yaw = init_Yaw - 360;
}
}
}
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float norm, halfT;
float hx, hy, hz, bz, bx;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float Pitch, Roll, Yaw;
/*方便之后的程序使用，减少计算时间*/
//auxiliary variables to reduce number of repeated operations，
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;
/*归一化测量值，加速度计和磁力计的单位是什么都无所谓，因为它们在此被作了归一化处理*/
//normalise the measurements
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;
/*从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值），下面这个是从飞行器坐标系到世界坐标系的转换公式*/
//compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
/*计算地理坐标系下的磁场矢量bxyz（参考值）。
因为地理地磁水平夹角，我们已知是0度（抛去磁偏角的因素，固定向北），我定义by指向正北，所以by=某值，bx=0
但地理参考地磁矢量在垂直面上也有分量bz，地球上每个地方都是不一样的。
我们无法得知，也就无法用来融合（有更适合做垂直方向修正融合的加速度计），所以直接从测量值hz上复制过来，bz=hz。
磁场水平分量，参考值和测量值的大小应该是一致的(bx*bx) + (by*by)) = ((hx*hx) + (hy*hy))。
因为bx=0，所以就简化成(by*by) = ((hx*hx) + (hy*hy))。可算出by。这里修改by和bx指向可以定义哪个轴指向正北*/
bx = sqrtf((hx*hx) + (hy*hy));
//by = sqrtf((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)，下面这个是从世界坐标系到飞行器坐标系的转换公式(转置矩阵)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
/*我们把地理坐标系上的磁场矢量bxyz，转到机体上来wxyz。
因为bx=0，所以所有涉及到bx的部分都被省略了。同理by=0，所以所有涉及到by的部分也可以被省略，这根据自己定义那个轴指北有关。
类似上面重力vxyz的推算，因为重力g的az=1，ax=ay=0，所以上面涉及到gxgy的部分也被省略了
你可以看看两个公式：wxyz的公式，把by换成ay（0），把bz换成az（1），就变成了vxyz的公式了（其中q0q0+q1q1+q2q2+q3q3=1）。*/
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);
// wx = 2*by*(q1q2 + q0q3) + 2*bz*(q1q3 - q0q2);
// wy = 2*by*(0.5 - q1q1 - q3q3) + 2*bz*(q0q1 + q2q3);
// wz = 2*by*(q2q3 - q0q1) + 2*bz*(0.5 - q1q1 - q2q2);
//现在把加速度的测量矢量和参考矢量做叉积，把磁场的测量矢量和参考矢量也做叉积。都拿来来修正陀螺。
// error is sum of cross product between reference direction of fields and direction measured by sensors
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);
// // integral error scaled integral gain
// exInt = exInt + ex*Ki;
// eyInt = eyInt + ey*Ki;
// ezInt = ezInt + ez*Ki;
// // adjusted gyroscope measurements
// gx = gx + Kp*ex + exInt;
// gy = gy + Kp*ey + eyInt;
// gz = gz + Kp*ez + ezInt;
halfT=GET_NOWTIME(); //得到每次姿态更新的周期的一半
if(ex != 0.0f && ey != 0.0f && ez != 0.0f) //很关键的一句话，原算法没有
{
// integral error scaled integral gain
exInt = exInt + ex*Ki * halfT; //乘以采样周期的一半
eyInt = eyInt + ey*Ki * halfT;
ezInt = ezInt + ez*Ki * halfT;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
}
// integrate quaternion rate and normalise，四元数更新算法
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;
// normalise quaternion
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm; //w
q1 = q1 * norm; //x
q2 = q2 * norm; //y
q3 = q3 * norm; //z
Yaw = atan2(-2 * (q0 * q3 + q1 * q2) , q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw,绕z轴转动
Pitch = asin ( 2 * (q1 * q3 - q0 * q2)) * 57.3; // pitch,绕y轴转动
Roll = atan2( 2 * (q0 * q1 + q2 * q3) , 1 - 2 * (q1 * q1 + q2 * q2)) * 57.3; // roll,绕x轴转动l |
|
楼主: zksniper
[复制链接]
楼主
