|
发表于 2014-2-21 14:27:56
|
显示全部楼层
为什么我用楼主方法做出来的角度,和我用DMP(MPU6050传感芯片)做出来的, 角度值两个比较接近,就是正负相反.
楼主帮我看看哪里出了问题.谢谢!
/*******for angle cal************/
#define Kp 1.0f//2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.001f//0.005f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.002f // half the sample period
//float halfT;
//#define qu30 1073741824.0f
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
float AcceRatio = 16384.0; //加速度计比例系数
float GyroRatio = 131.0; //16.4;//32.8;// //陀螺仪比例系数
void Get_Tiltangle(void) //4ms 中断调用
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
// if(MPU6050_is_DRY())
// {
MPU6050_getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gyrox= (gx-mpuoffsetx)/GyroRatio;//*mpukx; // gx单位为度秒, ax单位为牛米平方秒
gyroy= (gy-mpuoffsety)/GyroRatio;//*mpuky;
gyroz= (gz-mpuoffsetz)/GyroRatio;//*mpukz;
accx= ax/AcceRatio;
accy= ay/AcceRatio;
accz= az/AcceRatio;
gyrox*=0.0174;gyroy*=0.0174;gyroz*=0.0174;//转换为弧度
// normalise the measurements 单位化四元数 取模
norm = sqrt(accx*accx + accy*accy + accz*accz);
accx = accx / norm;
accy = accy / norm;
accz = accz / norm;
// estimated direction of gravity 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// error is sum of cross product between reference direction of field and direction measured by sensor
// 误差和方向传感器测量参考方向之间的交叉乘积的总和
ex = (accy*vz - accz*vy);
ey = (accz*vx - accx*vz);
ez = (accx*vy - accy*vx);
// integral error scaled integral gain
// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
// 调整后的陀螺仪测量
gyrox = gyrox + Kp*ex + exInt;
gyroy = gyroy + Kp*ey + eyInt;
gyroz = gyroz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
// 整合四元数率和正常化
q0 = q0 + (-q1*gyrox - q2*gyroy - q3*gyroz)*halfT;
q1 = q1 + (q0*gyrox + q2*gyroz - q3*gyroy)*halfT;
q2 = q2 + (q0*gyroy - q1*gyroz + q3*gyrox)*halfT;
q3 = q3 + (q0*gyroz + q1*gyroy - q2*gyrox)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
} |
|