標(biāo)題: 飛控STM32程序 參考了匿名等程序 [打印本頁]
作者: LLLBBB    時(shí)間: 2017-5-15 13:10
標(biāo)題: 飛控STM32程序 參考了匿名等程序
分享一套基于32的程序，參考了匿名等程序

編譯,下載,運(yùn)行,連接飛控串口和FTDI串口,串口波特率500K,上位機(jī)打開高級(jí)收碼,在上位機(jī)飛控狀態(tài)標(biāo)簽可以看到變化的傳感器數(shù)據(jù),3D顯示可以跟隨roll和pitch的變化而變化,因?yàn)闆]有寫yaw的上傳,所以yaw保持零度...此時(shí)打開飛控波形按鈕,打開波形顯示頁面,并打開相應(yīng)波形開關(guān),1到3為加速度,4到6為陀螺儀,10和11為roll和pitch,就可以看到變化的波形.

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

  5. #define RtA                 57.324841                                //弧度到角度
  6. #define AtR            0.0174533                                //度到角度
  7. #define Acc_G         0.0011963                                //加速度變成G
  8. #define Gyro_G         0.0152672                                //角速度變成度
  9. #define Gyro_Gr        0.0002663                                
  10. #define FILTER_NUM 20

  12. S_INT16_XYZ ACC_AVG;                        //平均值濾波后的ACC
  13. S_FLOAT_XYZ GYRO_I;                                //陀螺儀積分
  14. S_FLOAT_XYZ EXP_ANGLE;                //期望角度
  15. S_FLOAT_XYZ DIF_ANGLE;                //期望角度與實(shí)際角度差
  16. S_FLOAT_XYZ Q_ANGLE;                        //四元數(shù)計(jì)算出的角度

  18. int16_t        ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM];        //加速度滑動(dòng)窗口濾波數(shù)組

  20. void Prepare_Data(void)
  21. {
  22.         static uint8_t filter_cnt=0;
  23.         int32_t temp1=0,temp2=0,temp3=0;
  24.         uint8_t i;
  25.         
  26.         MPU6050_Read();
  27.         MPU6050_Dataanl();
  28.         
  29.         ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;//更新滑動(dòng)窗口數(shù)組
  30.         ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;
  31.         ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;
  32.         for(i=0;i<FILTER_NUM;i++)
  33.         {
  34.                 temp1 += ACC_X_BUF[i];
  35.                 temp2 += ACC_Y_BUF[i];
  36.                 temp3 += ACC_Z_BUF[i];
  37.         }
  38.         ACC_AVG.X = temp1 / FILTER_NUM;
  39.         ACC_AVG.Y = temp2 / FILTER_NUM;
  40.         ACC_AVG.Z = temp3 / FILTER_NUM;
  41.         filter_cnt++;
  42.         if(filter_cnt==FILTER_NUM)        filter_cnt=0;
  43.         
  44.         GYRO_I.X += MPU6050_GYRO_LAST.X*Gyro_G*0.0001;//0.0001是時(shí)間間隔,兩次prepare的執(zhí)行周期
  45.         GYRO_I.Y += MPU6050_GYRO_LAST.Y*Gyro_G*0.0001;
  46.         GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.0001;
  47. }

  49. void Get_Attitude(void)
  50. {
  51.         IMUupdate(MPU6050_GYRO_LAST.X*Gyro_Gr,
  52.                                                 MPU6050_GYRO_LAST.Y*Gyro_Gr,
  53.                                                 MPU6050_GYRO_LAST.Z*Gyro_Gr,
  54.                                                 ACC_AVG.X,ACC_AVG.Y,ACC_AVG.Z);        //*0.0174轉(zhuǎn)成弧度
  55. }
  56. ////////////////////////////////////////////////////////////////////////////////
  57. #define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  58. #define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
  59. #define halfT 0.001f                   // half the sample period采樣周期的一半

  61. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
  62. float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
  63. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
  64. {
  65.   float norm;
  66. //  float hx, hy, hz, bx, bz;
  67.   float vx, vy, vz;// wx, wy, wz;
  68.   float ex, ey, ez;

  70.   // 先把這些用得到的值算好
  71.   float q0q0 = q0*q0;
  72.   float q0q1 = q0*q1;
  73.   float q0q2 = q0*q2;
  74. //  float q0q3 = q0*q3;
  75.   float q1q1 = q1*q1;
  76. //  float q1q2 = q1*q2;
  77.   float q1q3 = q1*q3;
  78.   float q2q2 = q2*q2;
  79.   float q2q3 = q2*q3;
  80.   float q3q3 = q3*q3;
  81.         
  82.         if(ax*ay*az==0)
  83.                  return;
  84.                
  85.   norm = sqrt(ax*ax + ay*ay + az*az);       //acc數(shù)據(jù)歸一化
  86.   ax = ax /norm;
  87.   ay = ay / norm;
  88.   az = az / norm;

  90.   // estimated direction of gravity and flux (v and w)              估計(jì)重力方向和流量/變遷
  91.   vx = 2*(q1q3 - q0q2);                                                                                                //四元素中xyz的表示
  92.   vy = 2*(q0q1 + q2q3);
  93.   vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  95.   // error is sum of cross product between reference direction of fields and direction measured by sensors
  96.   ex = (ay*vz - az*vy) ;                                                                    //向量外積在相減得到差分就是誤差
  97.   ey = (az*vx - ax*vz) ;
  98.   ez = (ax*vy - ay*vx) ;

  100.   exInt = exInt + ex * Ki;                                                                  //對(duì)誤差進(jìn)行積分
  101.   eyInt = eyInt + ey * Ki;
  102.   ezInt = ezInt + ez * Ki;

  104.   // adjusted gyroscope measurements
  105.   gx = gx + Kp*ex + exInt;                                                                                                   //將誤差PI后補(bǔ)償?shù)酵勇輧x，即補(bǔ)償零點(diǎn)漂移
  106.   gy = gy + Kp*ey + eyInt;
  107.   gz = gz + Kp*ez + ezInt;                                                                                           //這里的gz由于沒有觀測(cè)者進(jìn)行矯正會(huì)產(chǎn)生漂移，表現(xiàn)出來的就是積分自增或自減

  109.   // integrate quaternion rate and normalise                                                   //四元素的微分方程
  110.   q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  111.   q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  112.   q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  113. ……………………

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

所有資料51hei提供下載:
飛控程序.7z (259.85 KB, 下載次數(shù): 63)




作者: nsygogogo    時(shí)間: 2017-10-12 10:38
謝謝樓主
作者: 劉查理    時(shí)間: 2018-3-29 11:07
先馬克一下，到時(shí)候回來下載



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