久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 2105|回復: 0
打印 上一主題 下一主題
收起左側

MPU6050用51單片機角度融合怎么計算?

[復制鏈接]
跳轉到指定樓層
樓主
ID:954170 發表于 2021-10-19 17:08 | 只看該作者 回帖獎勵 |倒序瀏覽 |閱讀模式
50黑幣
我把mpu6050的計算程序移植到51單片機上面，程序沒有報錯，卻計算不出角度，煩請指導一下，或給出正確的計算函數程序如下：
  1. #include <REG52.H>
  2. #include <math.h>    //Keil library
  3. #include <stdio.h>   //Keil library
  4. #include <INTRINS.H>
  5. typedef unsigned char  uchar;
  6. typedef unsigned short ushort;
  7. typedef unsigned int   uint;
  8. typedef unsigned char  u8;
  9. //--聲明一些需要使用的變量--//
  10. uchar MPU6050_DATA[14];
  11. int Gyro_x,Gyro_y,Gyro_z;
  12. int Gyro_angle_x=0,Gyro_angle_y=0,Gyro_angle_z=0;
  13. int Acc_x,Acc_y,Acc_z;
  14. int Temp;

  16. int                xdata g_x=0,g_y=0,g_z=0;                                        //陀螺儀矯正參數
  17. float        xdata a_x=0,a_y=0;                                                        //角度矯正參數
  18. float        data  AngleX=0,AngleY=0;                                        //四元數解算出的歐拉角
  19. float        xdata Angle_gx=0,Angle_gy=0,Angle_gz=0;                //由角速度計算的角速率(角度制)
  20. float        xdata Angle_ax=0,Angle_ay=0,Angle_az=0;                //由加速度計算的加速度(弧度制)
  21. //****************************************
  22. // 定義51單片機端口
  23. //****************************************
  24. #define DataPort P0                //LCD1602數據端口
  25. sbit    SCL=P1^1;                        //IIC時鐘引腳定義
  26. sbit    SDA=P1^0;                        //IIC數據引腳定義
  27. sbit    LCM_RS=P3^5;                //LCD1602命令端口
  28. sbit    LCM_RW=P3^6;                //LCD1602命令端口
  29. sbit    LCM_EN=P3^4;                //LCD1602命令端口

  31. //****************************************
  32. // 定義MPU6050內部地址
  33. //****************************************
  34. #define        SMPLRT_DIV                0x19        //陀螺儀采樣率，典型值：0x07(125Hz)
  35. #define        CONFIG                        0x1A        //低通濾波頻率，典型值：0x06(5Hz)
  36. #define        GYRO_CONFIG                0x1B        //陀螺儀自檢及測量范圍，典型值：0x18(不自檢，2000deg/s)
  37. #define        ACCEL_CONFIG        0x1C        //加速計自檢、測量范圍及高通濾波頻率，典型值：0x01(不自檢，2G，5Hz)
  38. #define        ACCEL_XOUT_H        0x3B
  39. #define        ACCEL_XOUT_L        0x3C
  40. #define        ACCEL_YOUT_H        0x3D
  41. #define        ACCEL_YOUT_L        0x3E
  42. #define        ACCEL_ZOUT_H        0x3F
  43. #define        ACCEL_ZOUT_L        0x40
  44. #define        TEMP_OUT_H                0x41
  45. #define        TEMP_OUT_L                0x42
  46. #define        GYRO_XOUT_H                0x43
  47. #define        GYRO_XOUT_L                0x44
  48. #define        GYRO_YOUT_H                0x45
  49. #define        GYRO_YOUT_L                0x46
  50. #define        GYRO_ZOUT_H                0x47
  51. #define        GYRO_ZOUT_L                0x48
  52. #define        PWR_MGMT_1                0x6B        //電源管理，典型值：0x00(正常啟用)
  53. #define        WHO_AM_I                        0x75        //IIC地址寄存器(默認數值0x68，只讀)
  54. #define        SlaveAddress        0xD0        //IIC寫入時的地址字節數據，+1為讀取
  55. //****************************************
  56. //定義類型及變量
  57. //****************************************
  58. uchar dis[4];                                                        //顯示數字(-511至512)的字符數組
  59. int        dis_data;                                                //變量
  60. //int        Temperature,Temp_h,Temp_l;        //溫度及高低位數據
  61. //****************************************
  62. //函數聲明
  63. //****************************************
  64. void  delay(unsigned int k);                                                                                //延時
  65. //LCD相關函數
  66. void  InitLcd();                                                                                                                //初始化lcd1602
  67. void  lcd_printf(uchar *s,int temp_data);
  68. void  WriteDataLCM(uchar dataW);                                                                        //LCD數據
  69. void  WriteCommandLCM(uchar CMD,uchar Attribc);                                //LCD指令
  70. void  DisplayOneChar(uchar X,uchar Y,uchar DData);                        //顯示一個字符
  71. void  DisplayListChar(uchar X,uchar Y,uchar *DData,L);        //顯示字符串
  72. //MPU6050操作函數
  73. void  InitMPU6050();                                                                                                        //初始化MPU6050
  74. void  Delay5us();
  75. void  I2C_Start();
  76. void  I2C_Stop();
  77. void  I2C_SendACK(bit ack);
  78. bit   I2C_RecvACK();
  79. void  I2C_SendByte(uchar dat);
  80. uchar I2C_RecvByte();
  81. void  I2C_ReadPage();
  82. void  I2C_WritePage();
  83. void  display_ACCEL_x();
  84. void  display_ACCEL_y();
  85. void  display_ACCEL_z();
  86. uchar Single_ReadI2C(uchar REG_Address);                                                //讀取I2C數據
  87. void  Single_WriteI2C(uchar REG_Address,uchar REG_data);//向I2C寫入數據
  88. void Delay2us(void)
  89. {
  90.         u8 i;
  91.         i = 11;                // @24MHZ, 6 + 33 = 39T, 1.625us
  92.         while (--i);
  93. }
  94. void Read_MPU6050(u8 *buf)
  95. {
  96.         u8        i;
  97.         
  98.         I2C_Start();                  //起始信號
  99.         I2C_SendByte(SlaveAddress);   //發送設備地址+寫信號
  100.         I2C_SendByte(ACCEL_XOUT_H);    //內部寄存器地址，
  101.         I2C_Start();                   //起始信號
  102.         I2C_SendByte(SlaveAddress+1);  //發送設備地址+讀信號
  103.         for(i=0; i<13; i++)
  104.         {
  105.                 buf[i] = I2C_RecvByte();       //讀出寄存器數據
  106.                 SDA = 0;                    //寫應答信號
  107.                 SCL = 1;                    //拉高時鐘線
  108.                 Delay2us();                 //延時
  109.                 SCL = 0;                    //拉低時鐘線
  110.                 Delay2us();                 //延時
  111.         }
  112.                 buf[i] = I2C_RecvByte();        //最后一個字節
  113.                 SDA = 1;                    //寫非應答信號
  114.                 SCL = 1;                    //拉高時鐘線
  115.                 Delay2us();                 //延時
  116.                 SCL = 0;                    //拉低時鐘線
  117.                 Delay2us();                 //延時
  118.         I2C_Stop();                    //停止信號
  119. }



  123. //****************************************
  124. //整數轉字符串
  125. //****************************************

  127. void lcd_printf(uchar *s,int temp_data)
  128. {
  129.         if(temp_data<0)
  130.         {
  131.                 temp_data=-temp_data;
  132.                 *s='-';
  133.         }
  134.         else *s=' ';
  135.         *++s =temp_data/100+0x30;
  136.         temp_data=temp_data%100;     //取余運算
  137.         *++s =temp_data/10+0x30;
  138.         temp_data=temp_data%10;      //取余運算
  139.         *++s =temp_data+0x30;
  140. }
  141. //****************************************
  142. //延時
  143. //****************************************
  144. void delay(unsigned int k)
  145. {
  146.         unsigned int i,j;
  147.         for(i=0;i<k;i++)
  148.         {
  149.                 for(j=0;j<121;j++);
  150.         }
  151. }
  152. //****************************************
  153. //LCD1602初始化
  154. //****************************************
  155. void InitLcd()
  156. {
  157.         WriteCommandLCM(0x38,1);
  158.         WriteCommandLCM(0x08,1);
  159.         WriteCommandLCM(0x01,1);
  160.         WriteCommandLCM(0x06,1);
  161.         WriteCommandLCM(0x0c,1);
  162.         DisplayOneChar(0,0,'A');
  163.         DisplayOneChar(0,1,'G');
  164. }
  165. //****************************************
  166. //LCD1602寫允許
  167. //****************************************
  168. void WaitForEnable(void)
  169. {
  170.         DataPort=0xff;
  171.         LCM_RS=0;LCM_RW=1;_nop_();
  172.         LCM_EN=1;_nop_();_nop_();
  173.         while(DataPort&0x80);
  174.         LCM_EN=0;
  175. }
  176. //****************************************
  177. //LCD1602寫入命令
  178. //****************************************
  179. void WriteCommandLCM(uchar CMD,uchar Attribc)
  180. {
  181.         if(Attribc)WaitForEnable();
  182.         LCM_RS=0;LCM_RW=0;_nop_();
  183.         DataPort=CMD;_nop_();
  184.         LCM_EN=1;_nop_();_nop_();LCM_EN=0;
  185. }
  186. //****************************************
  187. //LCD1602寫入數據
  188. //****************************************
  189. void WriteDataLCM(uchar dataW)
  190. {
  191.         WaitForEnable();
  192.         LCM_RS=1;LCM_RW=0;_nop_();
  193.         DataPort=dataW;_nop_();
  194.         LCM_EN=1;_nop_();_nop_();LCM_EN=0;
  195. }
  196. //****************************************
  197. //LCD1602寫入一個字符
  198. //****************************************
  199. void DisplayOneChar(uchar X,uchar Y,uchar DData)
  200. {
  201.         Y&=1;
  202.         X&=15;
  203.         if(Y)X|=0x40;
  204.         X|=0x80;
  205.         WriteCommandLCM(X,0);
  206.         WriteDataLCM(DData);
  207. }
  208. //****************************************
  209. //LCD1602顯示字符串
  210. //****************************************
  211. void DisplayListChar(uchar X,uchar Y,uchar *DData,L)
  212. {
  213.         uchar ListLength=0;
  214.         Y&=0x1;
  215.         X&=0xF;
  216.         while(L--)
  217.         {
  218.                 DisplayOneChar(X,Y,DData[ListLength]);
  219.                 ListLength++;
  220.                 X++;
  221.         }
  222. }
  223. //**************************************
  224. //延時5微秒(STC90C52RC@12M)
  225. //不同的工作環境,需要調整此函數
  226. //當改用1T的MCU時,請調整此延時函數
  227. //**************************************
  228. void Delay5us()
  229. {
  230.         _nop_();_nop_();_nop_();_nop_();
  231.         _nop_();_nop_();_nop_();_nop_();
  232.         _nop_();_nop_();_nop_();_nop_();
  233.         _nop_();_nop_();_nop_();_nop_();
  234.         _nop_();_nop_();_nop_();_nop_();
  235.         _nop_();_nop_();_nop_();_nop_();
  236. }
  237. //**************************************
  238. //I2C起始信號
  239. //**************************************
  240. void I2C_Start()
  241. {
  242.             Delay2us();                 //延時
  243.         Delay2us();                 //延時
  244.         SDA = 1;                    //拉高數據線
  245.         SCL = 1;                    //拉高時鐘線
  246.         Delay2us();                 //延時
  247.         Delay2us();                 //延時
  248.         SDA = 0;                    //產生下降沿
  249.         Delay2us();                 //延時
  250.         SCL = 0;                    //拉低時鐘線
  251.         Delay2us();                 //延時
  252. }
  253. //**************************************
  254. //I2C停止信號
  255. //**************************************
  256. void I2C_Stop()
  257. {
  258.     Delay2us();                 //延時
  259.     SDA = 0;                    //拉低數據線
  260.     SCL = 1;                    //拉高時鐘線
  261.         Delay2us();                 //延時
  262.     SDA = 1;                    //產生上升沿
  263.         Delay2us();                 //延時
  264. }
  265. //**************************************
  266. //I2C發送應答信號
  267. //入口參數:ack (0:ACK 1:NAK)
  268. //**************************************
  269. void I2C_SendACK(bit ack)
  270. {
  271.     SDA = ack;                  //寫應答信號
  272.     SCL = 1;                    //拉高時鐘線
  273.     Delay5us();                 //延時
  274.     SCL = 0;                    //拉低時鐘線
  275.     Delay5us();                 //延時
  276. }
  277. //**************************************
  278. //I2C接收應答信號
  279. //**************************************
  280. bit I2C_RecvACK(void)
  281. {
  282.     SDA = 1;
  283.         Delay2us();                 //延時
  284.     SCL = 1;                    //拉高時鐘線
  285.     Delay2us();                 //延時
  286.     CY = SDA;                   //讀應答信號
  287.     SCL = 0;                    //拉低時鐘線
  288.     Delay2us();                 //延時
  289.     return CY;
  290. }
  291. //**************************************
  292. //向I2C總線發送一個字節數據
  293. //**************************************
  294. void I2C_SendByte(u8 dat)
  295. {
  296.     u8 i;
  297.     for (i=0; i<8; i++)         //8位計數器
  298.     {
  299.         dat <<= 1;              //移出數據的最高位
  300.         SDA = CY;               //送數據口
  301.         SCL = 1;                //拉高時鐘線
  302.         Delay2us();             //延時
  303.         SCL = 0;                //拉低時鐘線
  304.         Delay2us();             //延時
  305.     }
  306.     I2C_RecvACK();
  307. }
  308. //**************************************
  309. //從I2C總線接收一個字節數據
  310. //**************************************
  311. uchar I2C_RecvByte()
  312. {
  313.     u8 i;
  314.     u8 dat = 0;
  315.     SDA = 1;                    //使能內部上拉,準備讀取數據,
  316.     for (i=0; i<8; i++)         //8位計數器
  317.     {
  318.         dat <<= 1;
  319.         SCL = 1;                //拉高時鐘線
  320.         Delay2us();             //延時
  321.         dat |= SDA;             //讀數據
  322.         SCL = 0;                //拉低時鐘線
  323.         Delay2us();             //延時
  324.     }
  325.     return dat;
  326. }
  327. //**************************************
  328. //向I2C設備寫入一個字節數據
  329. //**************************************
  330. void Single_WriteI2C(u8 REG_Address,u8 REG_data)
  331. {
  332.     I2C_Start();                  //起始信號
  333.     I2C_SendByte(SlaveAddress);   //發送設備地址+寫信號
  334.     I2C_SendByte(REG_Address);    //內部寄存器地址，
  335.     I2C_SendByte(REG_data);       //內部寄存器數據，
  336.     I2C_Stop();                   //發送停止信號
  337. }
  338. //**************************************
  339. //從I2C設備讀取一個字節數據
  340. //**************************************
  341. u8 Single_ReadI2C(u8 REG_Address)
  342. {
  343.         u8 REG_data;
  344.         I2C_Start();                   //起始信號
  345.         I2C_SendByte(SlaveAddress);    //發送設備地址+寫信號
  346.         I2C_SendByte(REG_Address);     //發送存儲單元地址，從0開始
  347.         I2C_Start();                   //起始信號
  348.         I2C_SendByte(SlaveAddress+1);  //發送設備地址+讀信號
  349.         REG_data=I2C_RecvByte();       //讀出寄存器數據
  350.         
  351.         SDA = 1;                    //寫應答信號
  352.         SCL = 1;                    //拉高時鐘線
  353.         Delay2us();                 //延時
  354.         SCL = 0;                    //拉低時鐘線
  355.         Delay2us();                 //延時
  356.         
  357.         I2C_Stop();                    //停止信號
  358.         return REG_data;
  359. }
  360. //**************************************
  361. //初始化MPU6050
  362. //**************************************
  363. void InitMPU6050()
  364. {
  365.         Single_WriteI2C(PWR_MGMT_1, 0x00);        //解除休眠狀態
  366.         Single_WriteI2C(SMPLRT_DIV, 0x07);
  367.         Single_WriteI2C(CONFIG, 0x04);
  368.         Single_WriteI2C(GYRO_CONFIG, 0x08);
  369.         Single_WriteI2C(ACCEL_CONFIG, 0x08);
  370. }
  371. //**************************************
  372. //合成數據
  373. //**************************************
  374. int GetData(uchar REG_Address)
  375. {
  376.         char H,L;
  377.         H=Single_ReadI2C(REG_Address);
  378.         L=Single_ReadI2C(REG_Address+1);
  379.         return (H<<8)+L;   //合成數據
  380. }
  381. //**************************************
  382. //在1602上顯示10位數據
  383. //**************************************
  384. void Display10BitData(int value,uchar x,uchar y)
  385. {
  386.         value/=64;                                                        //轉換為10位數據
  387.         lcd_printf(dis, value);                        //轉換數據顯示
  388.         DisplayListChar(x,y,dis,4);        //啟始列，行，顯示數組，顯示長度
  389. }
  390. //*********************************************************************
  391. //****************角度計算*********************************************
  392. //*********************************************************************
  393. #define        pi                3.14159265f                           
  394. #define        Kp                0.8f                        
  395. #define        Ki                0.001f                        
  396. #define        halfT        0.004f           

  398. float idata q0=1,q1=0,q2=0,q3=0;   
  399. float idata exInt=0,eyInt=0,ezInt=0;  

  401. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
  402. {
  403.         float data norm;
  404.         float idata vx, vy, vz;
  405.         float idata ex, ey, ez;

  407.         norm = sqrt(ax*ax + ay*ay + az*az);        
  408.         ax = ax / norm;
  409.         ay = ay / norm;
  410.         az = az / norm;

  412.         
  413.         vx = 2*(q1*q3 - q0*q2);
  414.         vy = 2*(q0*q1 + q2*q3);
  415.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;

  417.         ex = (ay*vz - az*vy) ;
  418.         ey = (az*vx - ax*vz) ;
  419.         ez = (ax*vy - ay*vx) ;

  421.         exInt = exInt + ex * Ki;
  422.         eyInt = eyInt + ey * Ki;
  423.         ezInt = ezInt + ez * Ki;

  425.         gx = gx + Kp*ex + exInt;
  426.         gy = gy + Kp*ey + eyInt;
  427.         gz = gz + Kp*ez + ezInt;

  429.         q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
  430.         q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;
  431.         q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;
  432.         q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;

  434.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  435.         q0 = q0 / norm;
  436.         q1 = q1 / norm;
  437.         q2 = q2 / norm;
  438.         q3 = q3 / norm;

  440.         AngleX = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f;
  441.         AngleY = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f;
  442. }
  443. //*********************************************************
  444. //主程序
  445. //*********************************************************
  446. void main()
  447. {               
  448.         delay(500);                //上電延時
  449.         InitLcd();                //液晶初始化
  450.         InitMPU6050();        //初始化MPU6050

  452.         delay(150);

  454.         while(1)
  455.         {                   Read_MPU6050(MPU6050_DATA);
  456.                 Acc_x = MPU6050_DATA[0]<<8|MPU6050_DATA[1];
  457.                 Acc_y = MPU6050_DATA[2]<<8|MPU6050_DATA[3];
  458.                 Acc_z = MPU6050_DATA[4]<<8|MPU6050_DATA[5];
  459.                 Temp  = MPU6050_DATA[6]<<8|MPU6050_DATA[7];
  460.                 Gyro_x = MPU6050_DATA[8]<<8|MPU6050_DATA[9];
  461.                 Gyro_y = MPU6050_DATA[10]<<8|MPU6050_DATA[11];
  462.                 Gyro_z = MPU6050_DATA[12]<<8|MPU6050_DATA[13];
  463.                
  464.                 Angle_ax = Acc_x/8192.0;
  465.                 Angle_ay = Acc_y/8192.0;
  466.                 Angle_az = Acc_z/8192.0;
  467.                
  468.                 Angle_gx = Gyro_x/65.5;
  469.                 Angle_gy = Gyro_y/65.5;
  470.                 Angle_gz = Gyro_z/65.5;
  471.                
  472.                 IMUupdate(Angle_gx*0.0174533f,Angle_gy*0.0174533f,Angle_gz*0.0174533f,Angle_ax,Angle_ay,Angle_az);
  473.                 Display10BitData(GetData(ACCEL_XOUT_H),2,0);        //顯示X軸加速度
  474.                 Display10BitData(GetData(ACCEL_YOUT_H),7,0);        //顯示Y軸加速度
  475.                 Display10BitData(GetData(ACCEL_ZOUT_H),12,0);        //顯示Z軸加速度
  476.                 Display10BitData(AngleX,2,1);        //顯示俯仰角
  477.                 Display10BitData(AngleY,7,1);        //顯示橫滾角
  478.                 Display10BitData(GetData(GYRO_ZOUT_H),12,1);        //未改，顯示Z軸角速度
  479.                 delay(500);
  480.         }
  481. }
復制代碼


分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏 分享淘帖 頂頂 踩踩
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 色偷偷噜噜噜亚洲男人 | 国产一级淫片a直接免费看 免费a网站 | 国产福利资源在线 | 草草视频在线观看 | 青青久在线视频 | 久久久久免费观看 | 真人女人一级毛片免费播放 | 日韩视频免费看 | 日本一本在线 | 欧美综合一区 | 电影91久久久| 亚洲精品一区二区三区在线 | 韩日在线 | 成人午夜激情 | 午夜精品久久久久久不卡欧美一级 | 亚洲精品在线免费 | 一区二区三区中文字幕 | 黄色国产 | 欧美日本在线观看 | 国产欧美一区二区久久性色99 | 国产一区二区三区久久久久久久久 | 国产98在线 | 免费, | 毛片在线免费播放 | 超碰最新在线 | 色姑娘综合网 | 国产午夜精品久久久 | 日韩精品 电影一区 亚洲 | 精品欧美一区二区在线观看欧美熟 | 在线观看av网站 | 久久男女视频 | 91精品国产91久久久久久吃药 | 日韩欧美国产一区二区三区 | 国产午夜精品理论片a大结局 | 91高清免费 | 99久久精品国产毛片 | 在线成人一区 | 久久久视频在线 | 色婷婷一区 | 欧美激情国产日韩精品一区18 | 奇米四色影视 | 久久精品国产一区二区三区不卡 |