標(biāo)題: STM32代碼MPU6050的姿態(tài)結(jié)算,摘用cleanfight的姿態(tài)解算代碼 [打印本頁(yè)]
作者: chrain5    時(shí)間: 2018-1-20 10:53
標(biāo)題: STM32代碼MPU6050的姿態(tài)結(jié)算,摘用cleanfight的姿態(tài)解算代碼
STM32代碼MPU6050的姿態(tài)結(jié)算，摘用cleanfight的姿態(tài)解算代碼，，，效果可以

單片機(jī)源程序如下:
  1. #include "imu.h"
  2. #include "imu.h"
  3. #include  "math.h"
  4. #include "flycontro.h"

  6. #define SPIN_RATE_LIMIT 20

  8. #ifndef sq
  9. #define sq(x) ((x)*(x))
  10. #endif
  11. #define sin_approx(x)   sinf(x)
  12. #define cos_approx(x)   cosf(x)
  13. #define atan2_approx(y,x)   atan2f(y,x)
  14. #define acos_approx(x)      acosf(x)
  15. #define tan_approx(x)       tanf(x)
  16. // Use floating point M_PI instead explicitly.
  17. #define M_PIf       3.14159265358979323846f




  22. Attitudat_Struct Accel,Gyro,Angle; //MPU姿態(tài)數(shù)據(jù)
  23. Attitudat_Struct Gyro_Offset;  //陀螺儀零漂數(shù)據(jù)
  24. short Accel_X,Accel_Y,Accel_Z,Gyro_X,Gyro_Y,Gyro_Z;
  25. void MPU_GetAngleDat(void)
  26. {
  27.   int  Accel_Xtemp,Accel_Ytemp,Accel_Ztemp,Accel_Xtempout,Accel_Ytempout,Accel_Ztempout;
  28. /// static float Yawerr=0,lastYawerr=0;
  29.   MPU_Get_Accelerometer(&Accel_X,&Accel_Y,&Accel_Z);        //得到加速度傳感器數(shù)據(jù)
  30.   MPU_Get_Gyroscope(&Gyro_X,&Gyro_Y,&Gyro_Z);        //得到陀螺儀數(shù)據(jù)
  31.   
  32.         Accel_Xtemp  = Accel_X;
  33.         Accel_Ytemp  = Accel_Y;
  34.         Accel_Ztemp  = Accel_Z;

  36.   Prepare_Data(&Accel_Xtemp,&Accel_Ytemp,&Accel_Ztemp,&Accel_Xtempout ,&Accel_Ytempout,&Accel_Ztempout);

  38.         Accel.X = Accel_Xtempout;     ////8192
  39.         Accel.Y = Accel_Ytempout;
  40.   Accel.Z = Accel_Ztempout;        
  41.        
  42.         Gyro.X = (Gyro_X - Gyro_Offset.X) ;
  43.         Gyro.Y = (Gyro_Y - Gyro_Offset.Y) ;
  44.         Gyro.Z = (Gyro_Z - Gyro_Offset.Z) ;

  46. }


  49. u8 Gyro_calibration(void)
  50. {
  51. //u16 i;
  52. //short  Gyro_X,Gyro_Y,Gyro_Z;
  53. static long int GyroofsetX=0,GyroofsetY=0,GyroofsetZ=0;
  54. static u16 i= 0;
  55. //        for(i=0;i<1000;i++)
  56. //         {
  57. // MPU_Get_Gyroscope(&Gyro_X,&Gyro_Y,&Gyro_Z);        //得到陀螺儀數(shù)據(jù)
  58.     GyroofsetX+=Gyro_X;
  59.                 GyroofsetY+=Gyro_Y;
  60.                 GyroofsetZ+=Gyro_Z;
  61.           i++;
  62. //                delay_ms(5);
  63. //   }
  64. //       
  65.         if(i==1000)
  66.         {
  67.    i=0;
  68.    Gyro_Offset.X = GyroofsetX/1000;
  69.    Gyro_Offset.Y = GyroofsetY/1000;
  70.    Gyro_Offset.Z = GyroofsetZ/1000;
  71.          GyroofsetX=GyroofsetY=GyroofsetZ=0;
  72.                 return 1;
  73.         }

  75.         return 0;
  76. }


  79. void Prepare_Data(int *accx,int *accy,int *accz,int *accoutx,int *accouty,int *accoutz)
  80. {  
  81.         static uint8_t         filter_cnt=0;
  82.         static int16_t        ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM];
  83.         int32_t temp1=0,temp2=0,temp3=0;
  84.         uint8_t i;       
  85.         ACC_X_BUF[filter_cnt] = *accx;
  86.         ACC_Y_BUF[filter_cnt] = *accy;
  87.         ACC_Z_BUF[filter_cnt] = *accz;
  88.         for(i=0;i<FILTER_NUM;i++)//滑動(dòng)平滑濾波
  89.         {
  90.                 temp1 += ACC_X_BUF[i];
  91.                 temp2 += ACC_Y_BUF[i];
  92.                 temp3 += ACC_Z_BUF[i];
  93.         }
  94.         *accoutx = temp1 / FILTER_NUM;
  95.         *accouty = temp2 / FILTER_NUM;
  96.         *accoutz = temp3 / FILTER_NUM;
  97.         filter_cnt++;
  98.         if(filter_cnt==FILTER_NUM)        filter_cnt=0;

  100. }












  113. float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
  114. static float rMat[3][3];

  116. float dcmKiGain = 0.005;
  117. float dcmKpGain = 2.0;

  119. static float invSqrt(float x)
  120. {
  121.     return 1.0f / sqrtf(x);
  122. }

  124. void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
  125.                                                                                                         bool useAcc, float ax, float ay, float az,
  126.                                                                                                         bool useMag, float mx, float my, float mz,
  127.                                                                                                         bool useYaw, float yawError)
  128. {
  129.     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
  130.     float recipNorm;
  131.     float hx, hy, bx;
  132.     float ex = 0, ey = 0, ez = 0;
  133.     float qa, qb, qc;
  134.    
  135.     // Calculate general spin rate (rad/s)
  136.     float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
  137.           float ez_ef;
  138.     // Use raw heading error (from GPS or whatever else)
  139.     if (useYaw) {
  140.         while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
  141.         while (yawError < -M_PIf) yawError += (2.0f * M_PIf);

  143.         ez += sin_approx(yawError / 2.0f);
  144.     }

  146.     // Use measured magnetic field vector  
  147.     recipNorm = sq(mx) + sq(my) + sq(mz);
  148.     if (useMag && recipNorm > 0.01f)
  149.                         {
  150.         // Normalise magnetometer measurement ?????????
  151.         recipNorm = invSqrt(recipNorm);
  152.         mx *= recipNorm;
  153.         my *= recipNorm;
  154.         mz *= recipNorm;
  155.                                 // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
  156.                                 // This way magnetic field will only affect heading and wont mess roll/pitch angles

  158.                                 // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
  159.                                 // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
  160.         hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
  161.         hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
  162.         bx = sqrtf(hx * hx + hy * hy);

  164.         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
  165.         ez_ef = -(hy * bx);

  167.         // Rotate mag error vector back to BF and accumulate
  168.         ex -= rMat[2][0] * ez_ef;
  169.         ey -= rMat[2][1] * ez_ef;
  170.         ez += rMat[2][2] * ez_ef;
  171.     }

  173.     // Use measured acceleration vector ?????????
  174.     recipNorm = sq(ax) + sq(ay) + sq(az);
  175.     if (useAcc && recipNorm > 0.01f)
  176.                         {
  177.         // Normalise accelerometer measurement ?????????
  178.         recipNorm = invSqrt(recipNorm);
  179.         ax *= recipNorm;
  180.         ay *= recipNorm;
  181.         az *= recipNorm;

  183.         // Error is sum of cross product between estimated direction and measured direction of gravity
  184.         ex += (ay * rMat[2][2] - az * rMat[2][1]);
  185.         ey += (az * rMat[2][0] - ax * rMat[2][2]);
  186.         ez += (ax * rMat[2][1] - ay * rMat[2][0]);
  187.     }

  189.     // Compute and apply integral feedback if enabled
  190.     if(dcmKiGain > 0.0f) {   //imuRuntimeConfig->dcm_ki
  191.         // Stop integrating if spinning beyond the certain limit????,??????????
  192.         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
  193.        //     float dcmKiGain = dcm_ki;
  194.             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
  195.             integralFBy += dcmKiGain * ey * dt;
  196.             integralFBz += dcmKiGain * ez * dt;
  197.         }
  198.     }
  199.     else {
  200.         integralFBx = 0.0f;    // prevent integral windup
  201.         integralFBy = 0.0f;
  202.         integralFBz = 0.0f;
  203.         }

  205.     // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
  206.     //   dcmKpGain =  dcm_kp ;//* imuGetPGainScaleFactor();

  208.     // Apply proportional and integral feedback??????
  209.     gx += dcmKpGain * ex + integralFBx;
  210.     gy += dcmKpGain * ey + integralFBy;
  211.     gz += dcmKpGain * ez + integralFBz;

  213.     // Integrate rate of change of quaternion??????
  214.     gx *= (0.5f * dt);
  215.     gy *= (0.5f * dt);
  216.     gz *= (0.5f * dt);

  218.     qa = q0;
  219.     qb = q1;
  220.     qc = q2;
  221.     q0 += (-qb * gx - qc * gy - q3 * gz);
  222.     q1 += (qa * gx + qc * gz - q3 * gy);
  223.     q2 += (qa * gy - qb * gz + q3 * gx);
  224.     q3 += (qa * gz + qb * gy - qc * gx);
  225.     // Normalise quaternion
  226.     recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
  227.     q0 *= recipNorm;
  228.     q1 *= recipNorm;
  229.     q2 *= recipNorm;
  230.     q3 *= recipNorm;

  232.     // Pre-compute rotation matrix from quaternion
  233.     imuComputeRotationMatrix();
  234. #if 0         
  235. //                 Angle.Y = asin(-2 * q1 * q3  + 2 * q0* q2)* 57.3 ; // pitch  
  236. //          Angle.X = atan2( 2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
  237. //          Angle.Z = atan2(2 * q1 * q2  + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
  238. #else         
  239.          imuUpdateEulerAngles(&Angle.X,&Angle.Y,&Angle.Z);
  240. #endif

  242. }

  244. void imuComputeRotationMatrix(void)
  245. {
  246.     float q1q1 = sq(q1);
  247.     float q2q2 = sq(q2);
  248.     float q3q3 = sq(q3);

  250.     float q0q1 = q0 * q1;
  251.     float q0q2 = q0 * q2;
  252. ……………………

  254. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
imu.zip (3.66 KB, 下載次數(shù): 176)


作者: home8976    時(shí)間: 2018-8-22 14:33
過(guò)來(lái)看看
作者: lyjpla    時(shí)間: 2019-10-4 21:45
謝謝！非常有參考價(jià)值
作者: 楓葉-嵌入式    時(shí)間: 2019-10-6 11:25
這個(gè)資料很不錯(cuò)，正需要
作者: 18519187648    時(shí)間: 2022-9-17 17:09
你這怎么用啊，一些參數(shù)都沒(méi)寫(xiě)說(shuō)明
作者: lm7298    時(shí)間: 2022-9-19 15:35
資料很不錯(cuò)，正好需要，感謝



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1