<acronym id="xonnx"></acronym>
      <td id="xonnx"></td>
    1. <pre id="xonnx"></pre>

      1. 專注電子技術學習與研究
        當前位置:單片機教程網 >> STM32 >> 瀏覽文章

        MPU6050(初步調試代碼:度數相差1-2度)

        作者:祥瑞曉曉   來源:不詳   點擊數:  更新時間:2014年08月03日   【字體:

         ************************************************************************/

        #define  YCC_GYRO_GLOBALS
        #include "stm32f10x.h"
        #include "main.h"
        #include <stdio.h>
        #include <math.h>
        #include <string.h>
        //定義MPU6050內部地址********************
        #define SMPLRT_DV    0x19 //采樣率分頻,典型值:0x07(125Hz)
        #defineCONFIG0x1A //低通濾波頻率,典型值:0x06(5Hz)
        #define GYRO_CONFIG0x1B //陀螺儀自檢及測量范圍及高通濾波頻率,典型值:0x18(不自檢,2000deg/s)
        #define ACCEL_CONFIG0x1C //加速計自檢、測量范圍及高通濾波頻率,典型值:0x01(不自檢,2G,5Hz)
        #define AX_H0x3B //儲存最近的X,Y,Z軸加速度計測量值
        #defineAX_L0x3C
        #defineAY_H0x3D
        #defineAY_L0x3E
        #defineAZ_H0x3F
        #defineAZ_L0x40
        #defineTEMP_H0x41 //儲存最近溫度傳感器的測量值
        #define TEMP_L0x42
        #define GX_H0x43 //儲存最近的X,Y,Z軸陀螺儀感應器的測量值
        #define GX_L0x44
        #define GY_H0x45
        #define GY_L0x46
        #define GZ_H0x47
        #define GZ_L0x48
        #define PWR_M0x6B //電源管理,典型值:0x00(正常啟動)
        #define FIFO_COUNT_H0x72 //必須按從高到低的順序讀取
        #define FIFO_COUNT_L0x73
        #define WHO_AM_I0x75 //IIC地址寄存器(默認數值:0x68,只讀)
        #defineMPU6050_Addr    0xD0  //定義器件在IIC總線中的從地址,根據ALT  ADDRESS地址引腳不同修改
        //******角度參數************
        static float Angle_x,Angle_y,Angle_z;  //小車最終傾斜角度
        static float Accel_x,Accel_y,Accel_z;  //小車加速度計算角度 
        //****************************
        //加速度軸和陀螺儀軸的零點漂移,取4000個數據的平均值
        #define Ax_offset 0x03D2
        #define Ay_offset0xFED4
        #define Az_offset0x5956
        #define Gx_offset0x0011
        #define Gy_offset0xFFE3
        #define Gz_offset0xFFF5
        #define PI3.14
        #define gyro_time0.01  //s
        //******卡爾曼參數************                
        float Q_angle=0.001;  
        float Q_gyro=0.003;
        float R_angle=0.5;
        float dt=0.01;                          //dt為kalman濾波器采樣時間;
        float C_0 = 1.0;
        float Q_bias, Angle_err;
        float PCt_0, PCt_1, E;
        float K_0, K_1, t_0, t_1;
        float Pdot[4] ={0,0,0,0};
        float PP[2][2] = { { 1, 0 },{ 0, 1 } };
        //*********************************************************
        typedef struct
        {
        float ax;
        float ay;
        float az;
        float gx;
        float gy;
        float gz;
        float temp;
        }GYRO_INFO;
        static GYRO_INFO gyro_info;
         
        static void Single_Write(u8 register_add,u8 date)
        {
        I2C_Bus(I2C_DEV_MPU3050,I2CMD_IOTODEV,register_add,&date,1);
        }
        static s16 Single_Read(u8 register_add)
        {
        u16 temp;
        u8 date[2];
        I2C_Bus(I2C_DEV_MPU3050,I2CMD_DEVTOIO,register_add,date,2);
        temp= (date[0]<<8)|date[1];
        return (s16)temp;
        }
        void Gyro_Init()
        {
           Single_Write(PWR_M, 0x00);   //重置所有寄存器,溫度傳感器失能
           Single_Write(SMPLRT_DV,0x07);//陀螺儀&加速度計采樣率,典型值:0x07(125Hz)
           Single_Write(CONFIG,0x06);   //低通濾波頻率,典型值:0x06(5Hz)
           Single_Write(GYRO_CONFIG,0x08);//(+-500deg/s
        //   Single_Write(GYRO_CONFIG,0x18);//陀螺儀自檢及測量范圍,典型值:0x18(不自檢,2000deg/s)
           Single_Write(ACCEL_CONFIG,0x01);//加速度計(不自檢,2g)
        }
         
        //******讀取MPU3050數據****************************************
        static u8 READ_MPU3050()
        {
        u8 power=0x40,ok;
        s16 temp[3];
        I2C_Bus(I2C_DEV_MPU3050,I2CMD_DEVTOIO,PWR_M,&power,1);
           if(power != 0x40)
           { 
              temp[0] = Single_Read(AX_H);
          temp[1] = Single_Read(AY_H);
          temp[2] = Single_Read(AZ_H);
               gyro_info.ax=(s16)(temp[0])/16384.00; //范圍為2g時,換算關系:16384 LSB/g
          gyro_info.ay=(s16)(temp[1])/16384.00; //角度較小時,x=sinx得到角度(弧度), deg = rad*180/3.14
          gyro_info.az=(s16)(temp[2])/16384.00; //因為x>=sinx,故乘以1.3適當放大
          gyro_info.gx=(s16)(Single_Read(GX_H)-Gx_offset)/65.5;///16.4; 
          gyro_info.gy=(s16)(Single_Read(GY_H)-Gy_offset)/65.5;///16.4;
          gyro_info.gz=(s16)(Single_Read(GZ_H)-Gz_offset)/65.5;///16.4;
        /*   Accel_x = atan(gyro_info.ay/gyro_info.az)*180/PI;
          Accel_y = atan(gyro_info.ax/gyro_info.az)*180/PI;*/
          Accel_x =asin((s16)(temp[0]-Ax_offset)/16384.00)*180/PI;
          Accel_y =asin((s16)(temp[1]-Ay_offset)/16384.00)*180/PI;
          Accel_z =asin((s16)(temp[2]-Az_offset)/16384.00)*180/PI;
          ok=1;
           }
           else 
           {
           Gyro_Init();
        ok=0;
           }
           return ok;
        }
        /*********************************************************/
        /*************傾角計算(卡爾曼濾波融合)********************/
        /*********************************************************/
        //Kalman濾波,20MHz的處理時間約0.77ms;
        void Kalman_Filter(float Accel,float* Gyro,float* outAngle)//,float*outAngleDot)
        {
        //static float Angle,AngleDot;
        *outAngle+=(*Gyro - Q_bias) * dt; //先驗估計
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分
        Pdot[1]=- PP[1][1];
        Pdot[2]=- PP[1][1];
        Pdot[3]=Q_gyro;
        PP[0][0] += Pdot[0] * dt;   // Pk-先驗估計誤差協方差微分的積分
        PP[0][1] += Pdot[1] * dt;   // =先驗估計誤差協方差
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
        Angle_err = Accel -*outAngle;//zk-先驗估計
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
        E = R_angle + C_0 * PCt_0;
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];
        PP[0][0] -= K_0 * t_0; //后驗估計誤差協方差
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
        *outAngle+= K_0 * Angle_err; //后驗估計
        Q_bias+= K_1 * Angle_err; //后驗估計
        *Gyro = *Gyro - Q_bias; //輸出值(后驗估計)的微分=角速度
         
        //*outAngle=*outAngle;
        //*outAngleDot=AngleDot;
        }
         
        /******************互補濾波**********************************
        *補償原理是取當前傾角和加速度獲得傾角差值進行放大,然后與
        * 陀螺儀角速度疊加后再積分,從而使傾角最跟蹤為加速度獲得的角度
        * 0.5為放大倍數,可調節補償度;gyro_time為系統周期10ms         
        **************************************************************/
         void Angle_Calcu(void) 
        {
        READ_MPU3050();
        Kalman_Filter(Accel_y,&gyro_info.gx,&Angle_x);
            Kalman_Filter(Accel_x,&gyro_info.gy,&Angle_y);
        //Kalman_Filter(Angle_z,Accel_z,gyro_info.gz);
        //Angle_x = Angle_x +(((Accel_y-Angle_x)*0.5 + gyro_info.gx)*gyro_time);
        //Angle_y = Angle_y +(((Accel_x-Angle_y)*0.5 + gyro_info.gy)*gyro_time);
        //Angle_z = Angle_z +(((Accel_x-Angle_y)*0.5 + gyro_info.gy)*gyro_time);
        }
        #ifdef DEBUG_MODE
        void Gyro_Report()
        {
        float* p;
        u8 i,len,temp[50];
        char str1[2]={'a','g'};
        char str2[3]={'x','y','z'};
        float Angle[3];
        p=(float*)(&gyro_info);
        //p=(s16*)(&gyro_offset);
        Angle[0]=Angle_x;
        Angle[1]=Angle_y;
        Angle[2]=Angle_z;
        if(I2C_Probe(I2C_DEV_MPU3050)==1)
        {
        for(i=0;i<6;i++)
        {
        if(i<3)  len=sprintf((char*)temp,"%c%c:%.2f",str1[0],str2[i],*p++);
        else     len=sprintf((char*)temp,"%c%c:%.2f",str1[1],str2[i-3],*p++);
        Host_Report_Event(HOST_EVENT_DEBUG,temp,len);
        }
        for(i=0;i<3;i++)
        {
        len=sprintf((char*)temp,"Angle%c:%.2f",str2[i],Angle[i]);
        Host_Report_Event(HOST_EVENT_DEBUG,temp,len);
        }
        }
        }
        #endif
         
        void Gyro_Main()
         
        Angle_Calcu();
         
        關閉窗口

        相關文章

        欧美性色欧美精品视频,99热这里只有精品mp4,日韩高清亚洲日韩精品一区二区,2020国自产拍精品高潮