搜索
bottom↓
回复: 4

四轴上MPU6050输出的 角度

[复制链接]

出0入0汤圆

发表于 2015-10-17 10:09:05 | 显示全部楼层 |阅读模式
////////////////////////////////////////////////////////////////////////////////
#define Kp 1.6f    // 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.001f  // 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
void IMUupdate(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 = sqrt(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 ;

  // 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由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减

  // 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 = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  //Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
  Q_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}


        printf("%f,  %f, %f\r\n",Q_ANGLE.X,Q_ANGLE.Y,Q_ANGLE.Z);


输出的数据为
-0.131456,  -0.013724, 0.000000
-0.263374,  -0.028662, 0.000000
-0.395509,  -0.044734, 0.000000
-0.528047,  -0.060314, 0.000000
-0.660972,  -0.075726, 0.000000
-0.794265,  -0.091475, 0.000000
-0.927926,  -0.107601, 0.000000
-1.061961,  -0.123908, 0.000000
-1.196487,  -0.140551, 0.000000
-1.331391,  -0.157320, 0.000000
-1.466669,  -0.174278, 0.000000
-1.602207,  -0.191161, 0.000000
-1.738127,  -0.208080, 0.000000
-1.874558,  -0.224975, 0.000000
-2.011240,  -0.242115, 0.000000
-2.148315,  -0.259065, 0.000000
-2.285766,  -0.276227, 0.000000
-2.423730,  -0.293104, 0.000000
-2.562085,  -0.309811, 0.000000
-2.700833,  -0.326357, 0.000000
-2.839844,  -0.342915, 0.000000
-2.979239,  -0.359487, 0.000000
-3.119020,  -0.376079, 0.000000
-3.259299,  -0.393030, 0.000000
-3.399840,  -0.409916, 0.000000
-3.540766,  -0.426851, 0.000000
-3.682204,  -0.443545, 0.000000
-3.823915,  -0.460309, 0.000000
-3.966011,  -0.476997, 0.000000
-4.108491,  -0.493864, 0.000000
-4.251471,  -0.510862, 0.000000
-4.394836,  -0.527907, 0.000000
-4.538482,  -0.544601, 0.000000
-4.682513,  -0.561326, 0.000000
-4.827062,  -0.577863, 0.000000
-4.971999,  -0.594431, 0.000000
-5.117208,  -0.610921, 0.000000
-5.262806,  -0.627414, 0.000000
-5.408802,  -0.643603, 0.000000
-5.555200,  -0.659305, 0.000000
-5.702114,  -0.674830, 0.000000
-5.849300,  -0.690081, 0.000000
-5.996873,  -0.705357, 0.000000
-6.144962,  -0.720588, 0.000000
-6.293439,  -0.735855, 0.000000
-6.442304,  -0.751280, 0.000000
-6.591542,  -0.767137, 0.000000
-6.740909,  -0.783393, 0.000000
-6.890782,  -0.799854, 0.000000
-7.041033,  -0.816521, 0.000000
-7.191552,  -0.833227, 0.000000
-7.342448,  -0.850152, 0.000000
-7.493840,  -0.867352, 0.000000
-7.645613,  -0.884757, 0.000000
-7.797788,  -0.901928, 0.000000
-7.950356,  -0.918933, 0.000000
-8.103330,  -0.935335, 0.000000
-8.256686,  -0.951912, 0.000000
-8.410430,  -0.968526, 0.000000
-8.564669,  -0.985521, 0.000000
-8.719168,  -1.002713, 0.000000
-8.874043,  -1.020121, 0.000000
-9.029313,  -1.037524, 0.000000
-9.184958,  -1.055056, 0.000000
-9.340851,  -1.073004, 0.000000
-9.497120,  -1.091153, 0.000000
-9.653870,  -1.109905, 0.000000
-9.811120,  -1.128854, 0.000000
-9.968749,  -1.148038, 0.000000
-10.126653,  -1.166900, 0.000000
-10.285078,  -1.185695, 0.000000
-10.443783,  -1.203994, 0.000000
-10.602891,  -1.221902, 0.000000
-10.762382,  -1.239988, 0.000000
-10.922261,  -1.257889, 0.000000
-11.082538,  -1.275349, 0.000000
-11.243311,  -1.293179, 0.000000
-11.404457,  -1.311236, 0.000000
-11.565885,  -1.328887, 0.000000
-11.727691,  -1.346567, 0.000000
-11.890002,  -1.364255, 0.000000
-12.052687,  -1.382306, 0.000000
-12.215621,  -1.400537, 0.000000
-12.378935,  -1.418947, 0.000000
-12.542743,  -1.437448, 0.000000
-12.706937,  -1.455761, 0.000000
-12.871393,  -1.474093, 0.000000
-13.036235,  -1.492250, 0.000000
-13.201580,  -1.510572, 0.000000
-13.367166,  -1.528921, 0.000000
-13.533119,  -1.547479, 0.000000
-13.699576,  -1.566292, 0.000000
-13.866415,  -1.585136, 0.000000
-14.033632,  -1.603699, 0.000000
-14.201235,  -1.622084, 0.000000
-14.369083,  -1.640558, 0.000000
-14.537318,  -1.658942, 0.000000
-14.705926,  -1.677340, 0.000000
-14.874914,  -1.695839, 0.000000
-15.044269,  -1.714529, 0.000000
-15.214259,  -1.732557, 0.000000
-15.382453,  -1.744894, 0.000000
-15.551012,  -1.757362, 0.000000
-15.719921,  -1.770307, 0.000000
-15.889315,  -1.783380, 0.000000
-16.059069,  -1.796286, 0.000000
-16.229050,  -1.809687, 0.000000
-16.399403,  -1.823100, 0.000000
-16.570255,  -1.836018, 0.000000
-16.741354,  -1.848875, 0.000000
-16.912666,  -1.864525, 0.000000
-16.790648,  -1.968822, 0.000000
-16.790648,  -1.968822, 0.000000
-16.790648,  -1.968822, 0.000000
-16.790648,  -1.968822, 0.000000
-16.790648,  -1.968822, 0.000000
-16.668701,  -1.861339, 0.000000
-16.523148,  -1.809449, 0.000000
-16.372728,  -1.785945, 0.000000
-16.372728,  -1.785945, 0.000000
-16.372728,  -1.785945, 0.000000
-16.222145,  -1.802332, 0.000000
-16.071690,  -1.818691, 0.000000
-16.071690,  -1.818691, 0.000000
-16.071690,  -1.818691, 0.000000
-16.071690,  -1.818691, 0.000000
-15.924963,  -1.858685, 0.000000
-15.924963,  -1.858685, 0.000000
-15.924963,  -1.858685, 0.000000
-15.803412,  -1.751316, 0.000000
-15.681940,  -1.644003, 0.000000
-15.546573,  -1.564199, 0.000000
-15.405076,  -1.501269, 0.000000
-15.269928,  -1.421320, 0.000000
-15.123805,  -1.377599, 0.000000
-14.977807,  -1.333822, 0.000000
-14.856876,  -1.226601, 0.000000
-14.736267,  -1.119356, 0.000000
-14.636189,  -0.984820, 0.000000
-14.553481,  -0.834324, 0.000000
-14.553481,  -0.834324, 0.000000
-14.553481,  -0.834324, 0.000000
-14.553481,  -0.834324, 0.000000
-14.553481,  -0.834324, 0.000000
-14.418907,  -0.754986, 0.000000
-14.418907,  -0.754986, 0.000000
-14.273572,  -0.796040, 0.000000
-14.126692,  -0.826647, 0.000000
-13.979832,  -0.857455, 0.000000
-13.839702,  -0.917498, 0.000000
-13.693236,  -0.948085, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.548707,  -0.988774, 0.000000
-13.646944,  -1.139607, 0.000000
-13.782932,  -1.247371, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.919066,  -1.355248, 0.000000
-13.963211,  -1.534729, 0.000000
-13.963211,  -1.534729, 0.000000
-13.963211,  -1.534729, 0.000000
-13.963211,  -1.534729, 0.000000
-13.880636,  -1.683513, 0.000000
-13.751777,  -1.772001, 0.000000
-13.631468,  -1.876146, 0.000000
-13.486889,  -1.915820, 0.000000
-13.339078,  -1.931679, 0.000000
-13.339078,  -1.931679, 0.000000
-13.191006,  -1.916288, 0.000000
-13.044243,  -1.888035, 0.000000
-12.900252,  -1.843465, 0.000000
-12.759537,  -1.786226, 0.000000
-12.613331,  -1.755558, 0.000000
-12.467400,  -1.724816, 0.000000
-12.321623,  -1.694096, 0.000000
-12.176126,  -1.663251, 0.000000
-12.031150,  -1.629101, 0.000000
-11.886330,  -1.594977, 0.000000
-11.751529,  -1.521382, 0.000000
-11.616866,  -1.447850, 0.000000
-11.474373,  -1.403774, 0.000000
-11.331912,  -1.359736, 0.000000
-11.331912,  -1.359736, 0.000000
-11.187034,  -1.383453, 0.000000
-11.042317,  -1.407110, 0.000000
-10.908354,  -1.476759, 0.000000
-10.790461,  -1.581099, 0.000000
-10.686151,  -1.705517, 0.000000
-10.560135,  -1.793744, 0.000000
-10.426623,  -1.863064, 0.000000
-10.298854,  -1.946742, 0.000000
-10.168016,  -2.022523, 0.000000
-10.031547,  -1.958758, 0.000000
-9.933875,  -1.823682, 0.000000
-9.816784,  -1.715889, 0.000000
-9.699917,  -1.608078, 0.000000
-9.582911,  -1.500229, 0.000000
-9.538289,  -1.326900, 0.000000
-9.457185,  -1.176025, 0.000000
-9.457185,  -1.176025, 0.000000
-9.457185,  -1.176025, 0.000000
-9.327258,  -1.096145, 0.000000
-9.185512,  -1.062459, 0.000000
-9.043253,  -1.035163, 0.000000
-8.908201,  -0.972409, 0.000000
-8.782106,  -0.885030, 0.000000
-8.640348,  -0.858038, 0.000000
-8.640348,  -0.858038, 0.000000
-8.502763,  -0.906509, 0.000000
-8.386841,  -1.011151, 0.000000
-8.279217,  -1.128586, 0.000000
-8.191384,  -1.270194, 0.000000
-8.133173,  -1.435577, 0.000000
-8.080112,  -1.603920, 0.000000
-8.012108,  -1.762483, 0.000000
-7.921564,  -1.900785, 0.000000
-7.821742,  -2.027921, 0.000000
-7.721860,  -2.154962, 0.000000
-7.606580,  -2.258433, 0.000000
-7.485633,  -2.350675, 0.000000
-7.354560,  -2.416133, 0.000000
-7.221360,  -2.473731, 0.000000
-7.088193,  -2.531242, 0.000000
-6.957567,  -2.596191, 0.000000
-6.827087,  -2.661180, 0.000000
-6.696638,  -2.726179, 0.000000
-6.575797,  -2.815817, 0.000000
-6.453925,  -2.902483, 0.000000
-6.330185,  -2.984572, 0.000000
-6.235015,  -3.115091, 0.000000
-6.285958,  -3.291287, 0.000000
-6.371961,  -3.446283, 0.000000
-6.423087,  -3.622976, 0.000000
-6.423087,  -3.622976, 0.000000
-6.423087,  -3.622976, 0.000000
-6.423087,  -3.622976, 0.000000
-6.308908,  -3.513189, 0.000000
-6.308908,  -3.513189, 0.000000
-6.308908,  -3.513189, 0.000000
-6.168969,  -3.522132, 0.000000
-6.029190,  -3.528571, 0.000000
-5.889594,  -3.534991, 0.000000
-5.750250,  -3.542552, 0.000000
-5.611075,  -3.551486, 0.000000
-5.611075,  -3.551486, 0.000000
-5.611075,  -3.551486, 0.000000
-5.611075,  -3.551486, 0.000000
-5.611075,  -3.551486, 0.000000
-5.611075,  -3.551486, 0.000000
-5.611075,  -3.551486, 0.000000
-5.479783,  -3.485410, 0.000000
-5.357207,  -3.395059, 0.000000
-5.244268,  -3.285445, 0.000000
-5.123840,  -3.191123, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-4.993218,  -3.125310, 0.000000
-5.064902,  -3.289432, 0.000000
-5.140250,  -3.451183, 0.000000
-5.224064,  -3.606504, 0.000000
-5.224064,  -3.606504, 0.000000
-5.224064,  -3.606504, 0.000000
-5.224064,  -3.606504, 0.000000
-5.179358,  -3.778473, 0.000000
-5.112180,  -3.935903, 0.000000
-4.999271,  -4.036973, 0.000000
-4.878872,  -4.121905, 0.000000
-4.743280,  -4.151910, 0.000000
-4.608828,  -4.187781, 0.000000
-4.478706,  -4.242733, 0.000000
-4.351857,  -4.308254, 0.000000
-4.239819,  -4.408760, 0.000000
-4.131975,  -4.516673, 0.000000
-4.024858,  -4.625352, 0.000000
-3.917834,  -4.734130, 0.000000
-3.821880,  -4.859290, 0.000000
-3.728286,  -4.987709, 0.000000
-3.631336,  -5.111443, 0.000000
-3.545804,  -5.249485, 0.000000
-3.467370,  -5.395036, 0.000000
-3.355982,  -5.494143, 0.000000
-3.244697,  -5.593101, 0.000000
-3.121534,  -5.663051, 0.000000
-2.998394,  -5.732994, 0.000000
-2.887569,  -5.831328, 0.000000
-2.764698,  -5.900850, 0.000000
-2.644974,  -5.978319, 0.000000
-2.522390,  -6.047566, 0.000000
-2.389664,  -6.074150, 0.000000
-2.260262,  -6.117627, 0.000000
-2.136923,  -6.182515, 0.000000
-2.022280,  -6.270388, 0.000000
-1.917562,  -6.377777, 0.000000
-1.794387,  -6.442184, 0.000000
-1.662647,  -6.467952, 0.000000
-1.529127,  -6.475651, 0.000000
-1.396199,  -6.443541, 0.000000
-1.264479,  -6.401166, 0.000000
-1.264479,  -6.401166, 0.000000
-1.138465,  -6.330494, 0.000000
-1.061303,  -6.176907, 0.000000
-0.995642,  -6.014696, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.945628,  -5.843599, 0.000000
-0.868788,  -5.690161, 0.000000
-0.868788,  -5.690161, 0.000000
-0.824420,  -5.516807, 0.000000
-0.695473,  -5.465818, 0.000000
-0.695473,  -5.465818, 0.000000
-0.695473,  -5.465818, 0.000000
-0.695473,  -5.465818, 0.000000
-0.695473,  -5.465818, 0.000000
-0.566735,  -5.498673, 0.000000
-0.441962,  -5.550678, 0.000000
-0.322621,  -5.620169, 0.000000
-0.278182,  -5.791044, 0.000000
-0.278182,  -5.791044, 0.000000
-0.278182,  -5.791044, 0.000000
-0.278182,  -5.791044, 0.000000
-0.353115,  -5.950642, 0.000000
-0.353115,  -5.950642, 0.000000
-0.353115,  -5.950642, 0.000000
-0.276626,  -6.095060, 0.000000
-0.168938,  -6.192416, 0.000000
-0.044726,  -6.243210, 0.000000
0.079323,  -6.293890, 0.000000
0.198331,  -6.361974, 0.000000
0.322161,  -6.412398, 0.000000
0.322161,  -6.412398, 0.000000
0.322161,  -6.412398, 0.000000
0.322161,  -6.412398, 0.000000
0.449528,  -6.443437, 0.000000
0.556516,  -6.540131, 0.000000
0.632469,  -6.683822, 0.000000
0.739159,  -6.780124, 0.000000
0.815023,  -6.923544, 0.000000
0.890842,  -7.066806, 0.000000
0.959051,  -7.217910, 0.000000
1.020718,  -7.374834, 0.000000
1.044684,  -7.555038, 0.000000
1.075655,  -7.732170, 0.000000
1.106750,  -7.909366, 0.000000
1.106750,  -7.909366, 0.000000
1.133585,  -8.088195, 0.000000
1.157501,  -8.268241, 0.000000
1.157501,  -8.268241, 0.000000
1.157501,  -8.268241, 0.000000
1.157501,  -8.268241, 0.000000
1.184634,  -8.446960, 0.000000
1.252927,  -8.596704, 0.000000
1.359325,  -8.689850, 0.000000
1.471364,  -8.769404, 0.000000
1.583414,  -8.848911, 0.000000
1.695331,  -8.928252, 0.000000
1.787901,  -9.045938, 0.000000
1.888006,  -9.149986, 0.000000
1.993851,  -9.242202, 0.000000
2.093889,  -9.345896, 0.000000
2.186151,  -9.462889, 0.000000
2.230622,  -9.631359, 0.000000
2.250571,  -9.812746, 0.000000
2.250571,  -9.812746, 0.000000
2.250571,  -9.812746, 0.000000
2.236829,  -10.000285, 0.000000
2.236829,  -10.000285, 0.000000
2.236829,  -10.000285, 0.000000
2.236829,  -10.000285, 0.000000
2.219439,  -10.187946, 0.000000
2.219439,  -10.187946, 0.000000
2.219439,  -10.187946, 0.000000
2.187331,  -10.373839, 0.000000
2.116485,  -10.540935, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.045312,  -10.708261, 0.000000
2.151362,  -10.798049, 0.000000
2.257305,  -10.887650, 0.000000
2.351969,  -10.998670, 0.000000
2.427439,  -11.136984, 0.000000
2.464249,  -11.309323, 0.000000
2.464249,  -11.309323, 0.000000
2.464249,  -11.309323, 0.000000
2.464249,  -11.309323, 0.000000
2.412575,  -11.489668, 0.000000
2.335406,  -11.652706, 0.000000
2.234160,  -11.785198, 0.000000
2.128287,  -11.909670, 0.000000
2.032037,  -12.051974, 0.000000
1.955137,  -12.217128, 0.000000
1.883212,  -12.386890, 0.000000
1.816899,  -12.560976, 0.000000
1.749307,  -12.734282, 0.000000
1.692291,  -12.913945, 0.000000
1.679218,  -13.102201, 0.000000
1.713584,  -13.275128, 0.000000
1.774665,  -13.427280, 0.000000
1.823161,  -13.590077, 0.000000
1.868377,  -13.755422, 0.000000
1.945109,  -13.890328, 0.000000
2.045625,  -13.988869, 0.000000
2.164231,  -14.043316, 0.000000
2.278664,  -14.110456, 0.000000
2.386050,  -14.194314, 0.000000
2.493181,  -14.277985, 0.000000
2.583428,  -14.392410, 0.000000
2.640489,  -14.546806, 0.000000
2.685616,  -14.711119, 0.000000
2.761850,  -14.844437, 0.000000
2.761850,  -14.844437, 0.000000
2.761850,  -14.844437, 0.000000
2.761850,  -14.844437, 0.000000
2.818529,  -14.677433, 0.000000
2.903432,  -14.527827, 0.000000
2.979597,  -14.371259, 0.000000
3.016975,  -14.199098, 0.000000
3.128910,  -14.087788, 0.000000
3.244303,  -13.985609, 0.000000
3.363703,  -13.896149, 0.000000
3.487816,  -13.826507, 0.000000
四轴板 是静止的
怎么输出的数据一直在抖动。。。

阿莫论坛20周年了!感谢大家的支持与爱护!!

一只鸟敢站在脆弱的枝条上歇脚,它依仗的不是枝条不会断,而是自己有翅膀,会飞。

出0入0汤圆

 楼主| 发表于 2015-10-17 10:10:21 | 显示全部楼层
到后来输出的数据变成了
42.400253,  -4.191398, 0.000000
42.429253,  -4.133758, 0.000000
42.429253,  -4.133758, 0.000000
42.429253,  -4.133758, 0.000000
42.429253,  -4.133758, 0.000000
42.457767,  -4.057551, 0.000000
42.484085,  -3.912763, 0.000000
42.512112,  -3.820593, 0.000000
42.540497,  -3.745285, 0.000000
42.569031,  -3.704885, 0.000000
42.597496,  -3.668670, 0.000000
42.625999,  -3.635552, 0.000000
42.654297,  -3.602604, 0.000000
42.682438,  -3.556211, 0.000000
42.710510,  -3.500008, 0.000000
42.710510,  -3.500008, 0.000000
42.710510,  -3.500008, 0.000000
42.710510,  -3.500008, 0.000000
42.710510,  -3.500008, 0.000000
42.710510,  -3.500008, 0.000000

出0入0汤圆

发表于 2015-10-17 10:23:42 | 显示全部楼层
四元数不对。感觉有滞后。

出0入0汤圆

发表于 2015-10-17 16:08:18 | 显示全部楼层
初始化对准的时候没动吧,或者你计算的时候是不是有误,还有就是你换个板子试试,可能是传感器不大好使了

出0入0汤圆

发表于 2015-10-20 16:25:25 | 显示全部楼层
楼主你的咋样了 我的也是这个情况
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。【本论坛不允许直接上传手机拍摄图片,浪费大家下载带宽和论坛服务器空间,请压缩后(图片小于1兆)才上传。压缩方法可以在微信里面发给自己(不要勾选“原图),然后下载,就能得到压缩后的图片】。另外,手机版只能上传图片,要上传附件需要切换到电脑版(不需要使用电脑,手机上切换到电脑版就行,页面底部)。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子技术论坛 ( 粤ICP备2022115958号, 版权所有:东莞阿莫电子贸易商行 创办于2004年 (公安交互式论坛备案:44190002001997 ) )

GMT+8, 2024-3-29 01:19

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表