TI论坛
直播中

马超

7年用户 156经验值
私信 关注
[问答]

mpu6050六轴传感器msp430驱动程序

    • #include
    • #include"mpu6050.h"
    • #include "USCI_A0.h"
    • #include "IMU.h "
    • /*
    • *  ======== BCSplus_graceInit ========
    • *  Initialize MSP430 Basic Clock System
    • */
    • void BCSplus_graceInit(void)
    • {
    •     /* USER CODE START (section: BCSplus_graceInit_prologue) */
    •     /* User initialization code */
    •     /* USER CODE END (section: BCSplus_graceInit_prologue) */
    •     /*
    •      * Basic Clock System Control 2
    •      *
    •      * SELM_0 -- DCOCLK
    •      * DIVM_0 -- Divide by 1
    •      * ~SELS -- DCOCLK
    •      * DIVS_1 -- Divide by 2
    •      * ~DCOR -- DCO uses internal resistor
    •      *
    •      * Note: ~ indicates that has value zero
    •      */
    •     BCSCTL2 = SELM_0 | DIVM_0 | DIVS_1;
    •     if (CALBC1_16MHZ != 0xFF) {
    •         /* Adjust this accordingly to your VCC rise time */
    •         __delay_cycles(100000);
    •         /* Follow recommended flow. First, clear all DCOx and MODx bits. Then
    •          * apply new RSELx values. Finally, apply new DCOx and MODx bit values.
    •          */
    •         DCOCTL = 0x00;
    •         BCSCTL1 = CALBC1_16MHZ;     /* Set DCO to 16MHz */
    •         DCOCTL = CALDCO_16MHZ;
    •     }
    •     /*
    •      * Basic Clock System Control 1
    •      *
    •      * XT2OFF -- Disable XT2CLK
    •      * ~XTS -- Low Frequency
    •      * DIVA_0 -- Divide by 1
    •      *
    •      * Note: ~XTS indicates that XTS has value zero
    •      */
    •     BCSCTL1 |= XT2OFF | DIVA_0;
    •     /*
    •      * Basic Clock System Control 3
    •      *
    •      * XT2S_0 -- 0.4 - 1 MHz
    •      * LFXT1S_0 -- If XTS = 0, XT1 = 32768kHz Crystal ; If XTS = 1, XT1 = 0.4 - 1-MHz crystal or resonator
    •      * XCAP_1 -- ~6 pF
    •      */
    •     BCSCTL3 = XT2S_0 | LFXT1S_0 | XCAP_1;
    •     /* USER CODE START (section: BCSplus_graceInit_epilogue) */
    •     /* User code */
    •     /* USER CODE END (section: BCSplus_graceInit_epilogue) */
    • }/*
    • * main.c
    • */
    • int main(void)
    • {
    •     WDTCTL = WDTPW | WDTHOLD;        // Stop watchdog timer
    •     BCSplus_graceInit();        //MCLK = 16M,SMCLK = 8M,ack =32.768k
    •     WDTCTL = WDTPW | WDTTMSEL | WDTIS0;
    •     IE1 |= WDTIE;
    •     USCI_A0_init();             //uart 38400bps  
    •     P1DIR |= BIT0;              //initialize led control PIN
    •     _EINT();
    •     MPU6050_Init();             //initialize mpu6050
    •    
    •     while(1)
    •     {
    •       /*
    •       Get_Attitude();
    •      //MPU6050_Dataanl();
    •      // ReadMPU6050All();
    •      //Prepare_Data();             //耗时6.15ms
    •      //Uart1_Send_AF();             //耗时8.42ms
    •      P1OUT ^= BIT0;
    •      Get_Attitude();
    •      //ReadMPU6050All();
    •      //MPU6050_Dataanl();         //未使用多字节读取，耗时6ms,使用多字节读取，耗时3.6ms
    •      //Prepare_Data();
    •     // Uart1_Send_AF();
    •      P1OUT ^= BIT0;
    •      */
    •     }
    •    
    • }
    • //watchdog interrupt server program per 1ms
    • #pragma vector=WDT_VECTOR
    • __interrupt void WDT_ISR_HOOK(void)
    • {
    •     /* USER CODE START (section: WDT_ISR_HOOK) */
    •     /* replace this comment with your code */
    •     static unsigned int count=0;
    •     static unsigned char ms2 = 0,ms5 = 0,ms10 = 0;
    •     count++;
    •     ms2++;
    •     ms5++;
    •     ms10++;
    •    
    •     if(ms2 == 2)
    •     {
    •      ms2 = 0;
    •      Prepare_Data();
    •      //MPU6050_Dataanl();
    •     }
    •     if(ms5 >= 4)
    •     {
    •       ms5 = 0;
    •       Get_Attitude();
    •    
    •     }
    •     if(ms10 >= 10)
    •     {
    •       ms10 = 0;
    •       Uart1_Send_AF();
    •     }
    •     if(count== 200)
    •     {
    •       count=0;
    •       P1OUT ^= BIT0;
    •     }
    •         /* USER CODE END (section: WDT_ISR_HOOK) */
    • }
    • /*
    • * This file contains some mpu6050 operation.
    • * By IC爬虫 (1394024051@qq.com)
    • * 2014-4-13 v1.0
    • */
    • #include "mpu6050.h"
    • unsigned char   mpu6050_buffer[14];     //I2C读取后存放数据
    • int ACC_OFFSET_X,ACC_OFFSET_Y,ACC_OFFSET_Z;
    • int GYRO_OFFSET_X,GYRO_OFFSET_Y,GYRO_OFFSET_Z;
    • unsigned char        GYRO_OFFSET_OK = 1;
    • unsigned char        ACC_OFFSET_OK = 1;
    • int MPU6050_ACC_LAST_X,MPU6050_ACC_LAST_Y,MPU6050_ACC_LAST_Z;    //final accelerate speed
    • int MPU6050_GYRO_LAST_X,MPU6050_GYRO_LAST_Y,MPU6050_GYRO_LAST_Z; //final  gyro  speed
    • /**********************************************************/
    • //函数名称：void MPU6050_Dataanl
    • //入口参数：无
    • //出口参数：无
    • //函数功能：MPU6050数据读取并处理
    • /**********************************************************/
    • void MPU6050_Dataanl(void)
    • {
    • #ifndef READALL
    •   MPU6050_ACC_LAST_X = GetAccelX() - ACC_OFFSET_X;
    •   MPU6050_ACC_LAST_Y = GetAccelY() - ACC_OFFSET_Y;
    •   MPU6050_ACC_LAST_Z = GetAccelZ() - ACC_OFFSET_Z;
    •   
    •   MPU6050_GYRO_LAST_X = GetAnguX() - GYRO_OFFSET_X;
    •   MPU6050_GYRO_LAST_Y = GetAnguY() - GYRO_OFFSET_Y;
    •   MPU6050_GYRO_LAST_Z = GetAnguZ() - GYRO_OFFSET_Z;
    •   //------------------------------------------------------------------//
    •   //补偿偏移
    •   if(!GYRO_OFFSET_OK)
    •   {
    •     static long int tempgx=0,tempgy=0,tempgz=0;
    •     static unsigned char cnt_g=0;
    •     if(cnt_g==0)
    •     {
    •       GYRO_OFFSET_X=0;
    •       GYRO_OFFSET_Y=0;
    •       GYRO_OFFSET_Z=0;
    •       tempgx = 0;
    •       tempgy = 0;
    •       tempgz = 0;
    •       cnt_g = 1;
    •     }
    •     tempgx+= MPU6050_GYRO_LAST_X;
    •     tempgy+= MPU6050_GYRO_LAST_Y;
    •     tempgz+= MPU6050_GYRO_LAST_Z;
    •     if(cnt_g==200)
    •     {
    •       GYRO_OFFSET_X=tempgx/cnt_g;
    •       GYRO_OFFSET_Y=tempgy/cnt_g;
    •       GYRO_OFFSET_Z=tempgz/cnt_g;
    •       cnt_g = 0;
    •       GYRO_OFFSET_OK = 1;
    •       
    •     }
    •     cnt_g++;
    •   }
    •   if(!ACC_OFFSET_OK)
    •   {
    •     static long int tempax=0,tempay=0,tempaz=0;
    •     static unsigned char cnt_a=0;
    •    
    •     if(cnt_a==0)
    •     {
    •       ACC_OFFSET_X = 0;
    •       ACC_OFFSET_Y = 0;
    •       ACC_OFFSET_Z = 0;
    •       tempax = 0;
    •       tempay = 0;
    •       tempaz = 0;
    •       cnt_a = 1;
    •       
    •     }
    •     tempax += MPU6050_ACC_LAST_X;//累加
    •     tempay += MPU6050_ACC_LAST_Y;
    •     tempaz += MPU6050_ACC_LAST_Z;
    •     if(cnt_a==200)
    •     {
    •       ACC_OFFSET_X = tempax/cnt_a;
    •       ACC_OFFSET_Y = tempay/cnt_a;
    •       ACC_OFFSET_Z = tempaz/cnt_a;
    •       cnt_a = 0;
    •       ACC_OFFSET_OK = 1;
    •       
    •     }
    •     cnt_a++;
    •   }
    •   //--------------------------------------------//
    • #else
    • struct MPU6050Struct *MPU6050WORK;
    •   MPU6050WORK = ReadMPU6050All();
    •   MPU6050_ACC_LAST_X = (MPU6050WORK ->MPU6050_ACC_X) - ACC_OFFSET_X;
    •   MPU6050_ACC_LAST_Y = (MPU6050WORK ->MPU6050_ACC_Y) - ACC_OFFSET_Y;
    •   MPU6050_ACC_LAST_Z = (MPU6050WORK ->MPU6050_ACC_Z) - ACC_OFFSET_Z;
    •   
    •   MPU6050_GYRO_LAST_X = (MPU6050WORK ->MPU6050_GYRO_X) - GYRO_OFFSET_X;
    •   MPU6050_GYRO_LAST_Y = (MPU6050WORK ->MPU6050_GYRO_Y) - GYRO_OFFSET_Y;
    •   MPU6050_GYRO_LAST_Z = (MPU6050WORK ->MPU6050_GYRO_Z) - GYRO_OFFSET_Z;
    •   if(!GYRO_OFFSET_OK)
    •   {
    •     static long int tempgx=0,tempgy=0,tempgz=0;
    •     static unsigned char cnt_g=0;
    •     if(cnt_g==0)
    •     {
    •       GYRO_OFFSET_X=0;
    •       GYRO_OFFSET_Y=0;
    •       GYRO_OFFSET_Z=0;
    •       tempgx = 0;
    •       tempgy = 0;
    •       tempgz = 0;
    •       cnt_g = 1;
    •     }
    •     tempgx+= MPU6050_GYRO_LAST_X;
    •     tempgy+= MPU6050_GYRO_LAST_Y;
    •     tempgz+= MPU6050_GYRO_LAST_Z;
    •     if(cnt_g==200)
    •     {
    •       GYRO_OFFSET_X=tempgx/cnt_g;
    •       GYRO_OFFSET_Y=tempgy/cnt_g;
    •       GYRO_OFFSET_Z=tempgz/cnt_g;
    •       cnt_g = 0;
    •       GYRO_OFFSET_OK = 1;
    •       
    •     }
    •     cnt_g++;
    •   }
    •   if(!ACC_OFFSET_OK)
    •   {
    •     static long int tempax=0,tempay=0,tempaz=0;
    •     static unsigned char cnt_a=0;
    •    
    •     if(cnt_a==0)
    •     {
    •       ACC_OFFSET_X = 0;
    •       ACC_OFFSET_Y = 0;
    •       ACC_OFFSET_Z = 0;
    •       tempax = 0;
    •       tempay = 0;
    •       tempaz = 0;
    •       cnt_a = 1;
    •       
    •     }
    •     tempax += MPU6050_ACC_LAST_X;//累加
    •     tempay += MPU6050_ACC_LAST_Y;
    •     tempaz += MPU6050_ACC_LAST_Z;
    •     if(cnt_a==200)
    •     {
    •       ACC_OFFSET_X = tempax/cnt_a;
    •       ACC_OFFSET_Y = tempay/cnt_a;
    •       ACC_OFFSET_Z = tempaz/cnt_a;
    •       cnt_a = 0;
    •       ACC_OFFSET_OK = 1;
    •       
    •     }
    •     cnt_a++;
    •   }
    • #endif
    •   
    • }
    • /**********************************************************/
    • //函数名称：void MPU6050Init
    • //入口参数：无
    • //出口参数：无
    • //函数功能：MPU6050初始化
    • /**********************************************************/
    • void MPU6050_Init()
    • {
    • #ifdef IMITATEIIC
    •   InitImitateIICPort();
    • #else
    •   I2C_Init(SlaveAddr);
    • #endif
    •   I2C_Write(PWR_MGMT_1,0x00); //resume from sleep.
    •   I2C_Write(SMPLRT_DIV, 0x07);
    •   I2C_Write(CONFIG, 0x06);
    •   I2C_Write(GYRO_CONFIG, 0x18);
    •   I2C_Write(ACCEL_CONFIG, 0x01);
    • }
    • /**********************************************************/
    • //函数名称：int Get16Bit
    • //入口参数：address:读取数据的地址
    • //出口参数：无
    • //函数功能：获取MPU6050相应地址上的数据
    • /**********************************************************/
    • int Get16Bit (unsigned char  address)
    • {
    • #ifndef MULTIREAD
    •   unsigned char  ho,lo;
    •   int temp ;
    •   ho = I2C_Read(address);
    •   lo = I2C_Read(address+1);
    •   temp=ho;
    •   temp<<=8;
    •   temp+=lo;
    •   return temp ;
    • #else
    •   return( Double_Read_ADXL345(address));
    • #endif
    • }
    • /**********************************************************/
    • //函数名称：
    • //入口参数：无
    • //出口参数：无
    • //函数功能：获取MPU6050相应轴上的加速度数据
    • /**********************************************************/
    • // X/Y/Z-Axis Acceleration
    • int GetAccelX ()
    • {
    •   return Get16Bit(ACCEL_XOUT_H);
    • }
    • int GetAccelY ()
    • {
    •   return Get16Bit(ACCEL_YOUT_H);
    • }
    • int GetAccelZ ()
    • {
    •   return Get16Bit(ACCEL_ZOUT_H);
    • }
    • /**********************************************************/
    • //函数名称：
    • //入口参数：无
    • //出口参数：无
    • //函数功能：获取MPU6050相应轴上的角速度数据
    • /**********************************************************/
    • // X/Y/Z-Axis Angular velocity
    • int GetAnguX ()
    • {
    •   return Get16Bit(GYRO_XOUT_H);
    • }
    • int GetAnguY ()
    • {
    •   return Get16Bit(GYRO_YOUT_H);
    • }
    • int GetAnguZ ()
    • {
    •   return Get16Bit(GYRO_ZOUT_H);
    • }
    • #include"msp430iic.h"  
    • struct MPU6050Struct    MPU6050Data;
    • void InitImitateIICPort(void)
    • {
    •   SET_SDA_OUT;          //set  SDA PIN is out mode
    •   SDA_HIGH;             // set SDA PIN out is high
    •   SCL_HIGH;             //set SCL PIN is input mode ,pull up register to SCL PIN high
    • }
    • /**************************************
    • 起始信号
    • **************************************/
    • void ADXL345_Start(void)
    • {
    •     SET_SDA_OUT;
    •     SDA_HIGH;                    //拉高数据线
    •     SCL_HIGH;                    //拉高时钟线
    •     Delay5us();                 //延时
    •     SDA_LOW;                    //产生下降沿
    •     Delay5us();                 //延时
    •     SCL_LOW;                    //拉低时钟线
    • }
    • /**************************************
    • 停止信号
    • **************************************/
    • void ADXL345_Stop(void)
    • {
    •     SET_SDA_OUT;
    •     SDA_LOW;                    //拉低数据线
    •     SCL_HIGH;                    //拉高时钟线
    •     Delay5us();                 //延时
    •     SDA_HIGH;                    //产生上升沿
    •     Delay5us();                 //延时
    • }
    • /**************************************
    • 发送应答信号
    • 入口参数:ack (0:ACK 1:NAK)
    • **************************************/
    • void ADXL345_SendACK(unsigned char ack)
    • {
    •     SET_SDA_OUT;
    •     if(ack)
    •       SDA_HIGH;                //写NACK应答信号
    •     else
    •       SDA_LOW;                 //写ACK应答信号
    •     SCL_HIGH;                    //拉高时钟线
    •     Delay5us();                 //延时
    •     SCL_LOW;                    //拉低时钟线
    •     Delay5us();                 //延时
    • }
    • /**************************************
    • 接收应答信号
    • **************************************/
    • unsigned char ADXL345_RecvACK(void)
    • {
    •     unsigned char ack;
    •     //------------------//
    •     //一下两句切不可调换顺序，否则会导致时序错误
    •     SET_SDA_IN;
    •     SCL_HIGH;                    //拉高时钟线  
    •     //-----------------//
    •     Delay5us();                 //延时
    •     ack = SDA_IN;                   //读应答信号
    •     SCL_LOW;                    //拉低时钟线
    •     Delay5us();                 //延时
    •     return ack;
    • }
    • /**************************************
    • 向IIC总线发送一个字节数据
    • **************************************/
    • void ADXL345_Senduchar(unsigned char dat)
    • {
    •     unsigned char i,m;
    •     SET_SDA_OUT;
    •     for (i=8; i!=0; i--)         //8位计数器
    •     {
    •         m=dat & 0x80;           //移出数据的最高位
    •         if(m == 0x80)
    •             SDA_HIGH;
    •         else
    •             SDA_LOW;
    •         SCL_HIGH;                //拉高时钟线
    •         Delay5us();             //延时
    •         SCL_LOW;                //拉低时钟线
    •         dat=dat<<1;
    •         Delay5us();             //延时
    •     }
    •     ADXL345_RecvACK();
    • }
    • /**************************************
    • 从IIC总线接收一个字节数据
    • **************************************/
    • unsigned char  ADXL345_Recvuchar(void)
    • {
    •     unsigned char  i;
    •     unsigned char dat = 0;
    •     unsigned char m;
    •     SDA_HIGH;                    //使能内部上拉,准备读取数据,
    •     SET_SDA_IN;
    •     for (i=8; i!=0; i--)         //8位计数器
    •     {
    •         dat <<= 1;
    •         SCL_HIGH;                //拉高时钟线
    •         SET_SDA_IN;
    •         m = SDA_IN;
    •         if(m == I2C_SDA)
    •             dat = dat|0x01;
    •         Delay5us();             //延时
    •         SCL_LOW;                //拉低时钟线
    •         Delay5us();             //延时
    •     }
    •     return dat;
    • }
    • //******单字节写入*******************************************
    • void Single_Write_ADXL345(unsigned char REG_Address,unsigned char REG_data)
    • {
    •     ADXL345_Start();                  //起始信号
    •     ADXL345_Senduchar(SlaveAddress);   //发送设备地址+写信号
    •     ADXL345_Senduchar(REG_Address);    //内部寄存器地址，请参考中文pdf22页
    •     ADXL345_Senduchar(REG_data);       //内部寄存器数据，请参考中文pdf22页
    •     ADXL345_Stop();                   //发送停止信号
    • }
    • //********单字节读取*****************************************
    • unsigned char  Single_Read_ADXL345(unsigned char  REG_Address)
    • {  unsigned char REG_data=0;
    •     ADXL345_Start();                          //起始信号
    •     ADXL345_Senduchar(SlaveAddress);           //发送设备地址+写信号
    •     ADXL345_Senduchar(REG_Address);                   //发送存储单元地址，从0开始
    •     ADXL345_Start();                          //起始信号
    •     ADXL345_Senduchar(SlaveAddress+1);         //发送设备地址+读信号
    •     REG_data=ADXL345_Recvuchar();              //读出寄存器数据
    •     ADXL345_SendACK(1);                     //NACK
    •     ADXL345_Stop();                           //停止信号
    •     return REG_data;
    • }
    • //********多字节读取*****************************************
    • int  Double_Read_ADXL345(unsigned char  REG_Address)
    • {
    •     unsigned char ValueL=0;
    •     int Value=0;
    •     ADXL345_Start();                            //起始信号
    •     ADXL345_Senduchar(SlaveAddress);            //发送设备地址+写信号
    •     ADXL345_Senduchar(REG_Address);             //发送存储单元地址，从0开始
    •     ADXL345_Start();                            //起始信号
    •     ADXL345_Senduchar(SlaveAddress+1);          //发送设备地址+读信号
    •    
    •     Value=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     ValueL=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(1);                         //NACK
    •    
    •     ADXL345_Stop();                             //停止信号
    •     Value=(Value<<8)+ValueL;
    •     return Value;
    • }
    • struct MPU6050Struct *ReadMPU6050All()
    • {
    •   unsigned char TempAcc1=0,TempAcc2=0,TempAcc3=0,TempAcc4=0,TempAcc5=0,TempAcc6=0;
    •   unsigned char TempGyro1=0,TempGyro2=0,TempGyro3=0,TempGyro4=0,TempGyro5=0,TempGyro6=0;
    •     ADXL345_Start();                            //起始信号
    •     ADXL345_Senduchar(SlaveAddress);            //发送设备地址+写信号
    •     ADXL345_Senduchar(0x3B);                    //发送存储单元地址，从0x3b开始
    •     ADXL345_Start();                            //起始信号
    •     ADXL345_Senduchar(SlaveAddress+1);          //发送设备地址+读信号
    •    
    •     TempAcc2=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempAcc1=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempAcc4=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempAcc3=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempAcc6=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempAcc5=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     ADXL345_Recvuchar();                         //丢弃不连续地址的数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     ADXL345_Recvuchar();                         //丢弃不连续地址的数据
    •     ADXL345_SendACK(0);
    •    
    •     TempGyro2=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempGyro1=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(0);
    •    
    •     TempGyro4=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempGyro3=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(0);
    •    
    •     TempGyro6=ADXL345_Recvuchar();                  //读出寄存器数据
    •     ADXL345_SendACK(0);                         //ACK
    •    
    •     TempGyro5=ADXL345_Recvuchar();                 //读出寄存器数据
    •     ADXL345_SendACK(1);                         //NACK
    •    
    •     ADXL345_Stop();
    •     MPU6050Data.MPU6050_ACC_X=(TempAcc2<<8) + TempAcc1;
    •     MPU6050Data.MPU6050_ACC_Y=(TempAcc4<<8) + TempAcc3;
    •     MPU6050Data.MPU6050_ACC_Z=(TempAcc6<<8) + TempAcc5;
    •    
    •     MPU6050Data.MPU6050_GYRO_X=(TempGyro2<<8) + TempGyro1;
    •     MPU6050Data.MPU6050_GYRO_Y=(TempGyro4<<8) + TempGyro3;
    •     MPU6050Data.MPU6050_GYRO_Z=(TempGyro6<<8) + TempGyro5;
    •     return (&MPU6050Data);
    • }
    • /*
    • * This file contains some uSCI_A0 operation.
    • * By IC爬虫 (1394024051@qq.com)
    • * 2014-4-28 v1.0
    • */
    • //#include "msp430g2553.h"
    • #include "USCI_A0.h"
    • //#include "stdio.h"
    • #include "mpu6050.h"
    • #include "IMU.h"
    • #define uchar unsigned char
    • #define uint unsigned int
    • //将“int”类型的数据分成两个单字节的数据
    • #define BYTE0(dwTemp)       (*(char *)(&dwTemp))
    • #define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))
    • #define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))
    • #define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))
    • /*********************************************************
    • *名称：USCI_A0_init
    • *功能：串口初始化
    • *入口参数：无
    • *出口参数：无
    • *说明：设置为P1.1和P1.2为串口通信端口
    • **********************************************************/
    • void USCI_A0_init(void)
    • {
    •   P1SEL = BIT1 + BIT2 ;   // P1.1 = RXD, P1.2=TXD
    •   P1SEL2 = BIT1 + BIT2;                     
    •   UCA0CTL1 |= UCSSEL_2;   // SMCLK
    •   /*
    •   UCA0BR0 = 0x45;         // 8MHz 115200
    •   UCA0BR1 = 0;            // 8MHz 115200
    •   UCA0MCTL = 0x4a;        // 8MHz 115200  */   
    •   /*
    •   UCA0BR0 = 0x68;
    •   UCA0BR1 = 0;
    •   UCA0MCTL = 0x40;
    •   */
    •   UCA0MCTL = UCBRF_0 | UCBRS_4;
    •    
    •     /* Baud rate control register 0 */
    •     UCA0BR0 = 69;
    •   UCA0CTL1 &= ~UCSWRST;          // **Initialize USCI state machine**
    •   
    •   //IE2 |= UCA0RXIE + UCA0TXIE;  // Enable USCI_A0 TX/RX interrupt
    •   //IE2 |= UCA0RXIE;             // Enable USCI_A0 RX interrupt
    •   //__bis_SR_register(GIE);      // Enter LPM3 w/ interrupts enabled
    • }
    • /*********************************************************
    • *名称：UartTX_Send_String
    • *功能：串口发送字符串函数
    • *入口参数：*data:数据指针        len :数据长度
    • *出口参数：无
    • *说明：
    • **********************************************************/
    • void UartTX_Send_String(unsigned char *Data,int len)
    • {
    •   int j;
    •   for(j=0;j
    •   {
    •     UartTX_Send_char(*Data++);
    •   }
    • }
    • /*********************************************************
    • *名称：UartTX_Send_char
    • *功能：串口发送字符函数
    • *入口参数：c
    • *出口参数：无
    • *说明：
    • **********************************************************/
    • unsigned char UartTX_Send_char(unsigned char c)
    • {
    •     UCA0TXBUF=c;   
    •     while(!(IFG2&UCA0TXIFG));
    •     IFG2&=~UCA0TXIFG;
    •     return c;
    • }
    • /*********************************************************
    • *名称：int putchar
    • *功能：串口发送字符函数
    • *入口参数：ch
    • *出口参数：无
    • *说明：
    • **********************************************************/
    • int putchar(int ch)
    • {
    •   UCA0TXBUF=ch;
    •   while(!(IFG2&UCA0TXIFG));
    •    //UCA0TXBUF=ch;
    •   IFG2&=~UCA0TXIFG;
    •    return ch;
    • }
    • void sendChar(unsigned char c)
    • {
    •    while(!(IFG2&UCA0TXIFG));
    •    UCA0TXBUF=c;
    • }
    • void sendStr(unsigned char *s)
    • {
    •   while(*s!='\0')
    •   {
    •     sendChar(*s);
    •     s++;
    •   }
    • }
    • /*********************************************************
    • *名称：void Uart1_Send_AF
    • *功能：串口发送姿态数据
    • *入口参数：无
    • *出口参数：无
    • *说明：每一次执行这个函数就算是一帧数据，帧头为0X88，功能字
    • *      为0XAF
    • **********************************************************/
    • void Uart1_Send_AF(void)
    • {
    •   unsigned char sum = 0;//累加串口发送的数据的值，做校验用
    •   unsigned int _temp;   
    •   sum += UartTX_Send_char(0x88);  //帧头
    •   sum += UartTX_Send_char(0xAF);  //功能字
    •   
    •   sum += UartTX_Send_char(0x1c);
    •   sum += UartTX_Send_char( BYTE1(MPU6050_ACC_LAST_X) ); //发送加速度X轴数据的高8位
    •   sum += UartTX_Send_char( BYTE0(MPU6050_ACC_LAST_X) ); //发送加速度X轴数据的低8位
    •   
    •   sum += UartTX_Send_char( BYTE1(MPU6050_ACC_LAST_Y) ); //发送加速度Y轴数据的高8位
    •   sum += UartTX_Send_char( BYTE0(MPU6050_ACC_LAST_Y) ); //发送加速度Y轴数据的低8位
    •   
    •   sum += UartTX_Send_char( BYTE1(MPU6050_ACC_LAST_Z) ); //发送加速度Z轴数据的高8位
    •   sum += UartTX_Send_char( BYTE0(MPU6050_ACC_LAST_Z) ); //发送加速度Z轴数据的低8位
    •   
    •   sum += UartTX_Send_char( BYTE1(MPU6050_GYRO_LAST_X) ); //发送陀螺仪X轴数据的高8位
    •   sum += UartTX_Send_char( BYTE0(MPU6050_GYRO_LAST_X) ); //发送陀螺仪X轴数据的低8位
    •   
    •   sum += UartTX_Send_char( BYTE1(MPU6050_GYRO_LAST_Y) ); //发送陀螺仪Y轴数据的高8位
    •   sum += UartTX_Send_char( BYTE0(MPU6050_GYRO_LAST_Y) ); //发送陀螺仪Y轴数据的低8位
    •   
    •   sum += UartTX_Send_char( BYTE1(MPU6050_GYRO_LAST_Z) ); //发送陀螺仪Z轴数据的高8位
    •   sum += UartTX_Send_char( BYTE0(MPU6050_GYRO_LAST_Z) ); //发送陀螺仪Z轴数据的低8位
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •         
    •   _temp = (long int)(Q_ANGLE_X*100);
    •   sum += UartTX_Send_char( BYTE1(_temp) );
    •   sum += UartTX_Send_char( BYTE0(_temp) );
    •   _temp = (long int)(Q_ANGLE_Y*100);
    •   sum += UartTX_Send_char( BYTE1(_temp) );
    •   sum += UartTX_Send_char( BYTE0(_temp) );
    •         
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   sum += UartTX_Send_char(0);
    •   
    •   UartTX_Send_char(sum); //串口发送累加值用于校验
    • }
    • /*
    • * This file contains some IMU operation.
    • * By IC爬虫 (1394024051@qq.com)
    • * 2014-4-29 v1.0
    • */
    • #include "IMU.h"
    • #define RtA           57.324841  //弧度到角度
    • #define AtR              0.0174533  //度到角度
    • #define Acc_G           0.0011963  //加速度变成G
    • #define Gyro_G           0.0152672  //角速度变成度
    • #define Gyro_Gr          0.0002663                                
    • #define FILTER_NUM 20
    • int   ACC_AVG_X,ACC_AVG_Y,ACC_AVG_Z;      //平均值滤波后的ACC
    • float GYRO_I_X,GYRO_I_Y,GYRO_I_Z;         //陀螺仪积分
    • float EXP_ANGLE_X,EXP_ANGLE_Y,EXP_ANGLE_Z;//期望角度
    • float DIF_ANGLE_X,DIF_ANGLE_Y,DIF_ANGLE_Z;//期望角度和实际角度的差
    • float Q_ANGLE_X,Q_ANGLE_Y,Q_ANGLE_Z;      //四元数计算出的角度
    • int ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM];        //加速度滑动窗口滤波数组
    • /**********************************************************/
    • //函数名称：Prepare_Data
    • //入口参数：无
    • //出口参数：无
    • //函数功能：读取MPU6050数据进行平滑滤波，为后续计算准备数据
    • /**********************************************************/
    • void Prepare_Data(void)
    • {
    •   static unsigned char filter_cnt=0;
    •   long int temp1=0,temp2=0,temp3=0;
    •   unsigned char i;
    •         
    •   MPU6050_Dataanl();//完成传感器数据的读取和计算，并且对数据简单处理
    •         
    •   ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST_X;//更新滑动窗口数组
    •   ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST_Y;
    •   ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST_Z;
    •   
    •   for(i=0;i
    •   {
    •     temp1 += ACC_X_BUF;
    •     temp2 += ACC_Y_BUF;
    •     temp3 += ACC_Z_BUF;
    •   }
    •   
    •   ACC_AVG_X = temp1 / FILTER_NUM;
    •   ACC_AVG_Y = temp2 / FILTER_NUM;
    •   ACC_AVG_Z = temp3 / FILTER_NUM;
    •   
    •   filter_cnt++;
    •   
    •   if(filter_cnt==FILTER_NUM)  filter_cnt=0;
    •         
    •   GYRO_I_X += MPU6050_GYRO_LAST_X*Gyro_G*0.02;//0.0001是时间间隔，两次prepare函数的执行周期
    •   GYRO_I_Y += MPU6050_GYRO_LAST_Y*Gyro_G*0.02;//示波器测量的得到的时间是20ms.
    •   GYRO_I_Z += MPU6050_GYRO_LAST_Z*Gyro_G*0.02;
    • }
    • void Get_Attitude(void)
    • {
    •   IMUupdate( MPU6050_GYRO_LAST_X*Gyro_Gr,
    •             MPU6050_GYRO_LAST_Y*Gyro_Gr,
    •             MPU6050_GYRO_LAST_Z*Gyro_Gr,
    •             ACC_AVG_X,ACC_AVG_Y,ACC_AVG_Z);        //*0.0174转成弧度
    • }
    • ////////////////////////////////////////////////////////////////////////////////
    • #define Kp 10.0f        // proportional gain governs rate of convergence to accelerometer/magnetometer
    • #define Ki 0.008f       // integral gain governs rate of convergence of gyroscope biases
    • #define halfT 0.004f    // 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
    • /**********************************************************/
    • //函数名称：IMUupdate
    • //入口参数：gx：浮点型的陀螺仪x轴数据
    • //          gy：浮点型的陀螺仪y轴数据
    • //          gz：浮点型的陀螺仪z轴数据
    • //          ax：浮点型的加速度x轴数据
    • //          ay：浮点型的加速度y轴数据
    • //          az：浮点型的加速度z轴数据
    • //出口参数：无
    • //函数功能：通过陀螺仪和加速度传感器的数据用四元数计算姿态
    • /**********************************************************/
    • 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
    • }
    • /*
    • * This file contains some I2C operation.
    • * By IC爬虫 (1394024051@qq.com)
    • * 2014-4-13 v1.0
    • */
    • #include "HardWareIIC.h"
    • uchar I2CSendBuffer[2],I2CRecvBuffer; //I2C发送缓存和接收缓存
    • int I2CSendPtr=0;
    • /**********************************************************/
    • //函数名称：void I2C_Init
    • //入口参数：SlaveAddr:从机的设备地址
    • //出口参数：无
    • //函数功能：I2C初始化，P1.6->SCL ,P1.7->SDA
    • /**********************************************************/
    • void I2C_Init (unsigned char SlaveAddr)
    • {
    •   P1SEL |= BIT6+BIT7;               // Assign I2C pins to USCI_B0
    •   P1SEL2|= BIT6+BIT7;               // Assign I2C pins to USCI_B0
    •   UCB0CTL1 |= UCSWRST;              // Enable SW reset
    •   
    •   UCB0CTL0 = UCMST+UCMODE_3+UCSYNC; // I2C Master, synchronous mode
    •   UCB0CTL0 &= ~(UCSLA10+UCA10);                        //7 bit add of slave and master
    •   
    •   UCB0CTL1 = UCSSEL_2+UCSWRST;      // Use SMCLK, keep SW reset
    •   
    •   UCB0BR0 = 80;                     // fSCL = SMCLK/12 = ~100kHz
    •   UCB0BR1 = 0;
    •   
    •   UCB0I2COA = 0x01A5;                                                                //set own address
    •   UCB0I2CSA = SlaveAddr;            // Set slave address
    •   
    •   IE2 &= ~(UCB0RXIE+UCB0TXIE);         // disenable  TX&RX interrupt
    •   UCB0CTL1 &= ~UCSWRST;             // Clear SW reset, resume operation
    •   
    • }
    • /**********************************************************/
    • //函数名称：void I2C_WriteInit
    • //入口参数：无
    • //出口参数：无
    • //函数功能：I2C写数据初始化，发送模式，接收中断关闭，发送中断关闭
    • /**********************************************************/
    • void I2C_WriteInit()
    • {
    •   UCB0CTL1 |= UCTR;            // UCTR=1 => Transmit Mode (R/W bit = 0)
    •   IFG2 &= ~UCB0TXIFG;                //clean TX interrupt sign
    •   IE2 &= ~UCB0RXIE;           // disable Receive ready interrupt
    •   IE2 &= ~UCB0TXIE;     // disable Transmit ready interrupt
    • }
    • /**********************************************************/
    • //函数名称：void I2C_ReadInit
    • //入口参数：无
    • //出口参数：无
    • //函数功能：I2C读数据初始化，接收模式，接收中断关闭，发送中断关闭
    • /**********************************************************/
    • void I2C_ReadInit()
    • {
    •   UCB0CTL1 &= ~UCTR;   // UCTR=0 => Receive Mode (R/W bit = 1)
    •   IFG2 &= ~UCB0RXIFG;
    •   IE2 &= ~UCB0TXIE;    // disable Transmit ready interrupt
    •   IE2 &= ~UCB0RXIE;     // disable Receive ready interrupt
    • }
    • /**********************************************************/
    • //函数名称：I2C_Write
    • //入口参数：address:需要写入数据的设备的地址
    • //          data：发送的数据
    • //出口参数：无
    • //函数功能：I2C发送数据
    • /**********************************************************/
    • void I2C_Write(uchar address,uchar data)
    • {
    •   I2C_WriteInit();
    •   UCB0CTL1 |= UCTXSTT;          //generate start condition
    •   //while(UCB0CTL1 & UCTXSTT);        //generate start condition ,and transmit slave address and write bit
    •   while(!(IFG2 & UCB0TXIFG));   //wait start condition and equipment address transmitted
    •   IFG2 &= ~UCB0TXIFG;           //clean UCB0TXIFG
    •   while(UCB0CTL1 & UCTXSTT);    //wait slave acknowledge
    •   
    •   UCB0TXBUF=address;                //send address code
    •   while(!(IFG2 & UCB0TXIFG ));        //wait sending over
    •   IFG2 &= ~UCB0TXIFG;            // Clear USCI_B0 TX int flag
    •    
    •   UCB0TXBUF=data;                //send  data
    •   while(!(IFG2 & UCB0TXIFG ));                //wait sending over
    •   IFG2 &= ~UCB0TXIFG;            // Clear USCI_B0 TX int flag
    •   
    •   UCB0CTL1 |= UCTXSTP;            // I2C stop condition
    •   while (UCB0CTL1 & UCTXSTP);     // Ensure stop condition got sent
    • }
    • /**********************************************************/
    • //函数名称：uchar I2C_Read
    • //入口参数：address:需要读数据的设备的地址
    • //出口参数：无
    • //函数功能：I2C接收数据
    • /**********************************************************/
    • uchar I2C_Read(uchar address)
    • {
    •   unsigned char data;
    •   while (UCB0STAT & UCBUSY); // wait until I2C module has finished all operations
    •   I2C_WriteInit();
    •   UCB0CTL1 |= UCTXSTT;       // start condition generation
    •   while(UCB0CTL1 & UCTXSTT);
    •   
    •   
    •   UCB0TXBUF=address;                                                        //send address code
    •   while(!(IFG2 & UCB0TXIFG ));                //wait sending over
    •   IFG2 &= ~UCB0TXIFG;            // Clear USCI_B0 TX int flag
    •   
    • // __disable_interrupt();
    • I2C_ReadInit();
    •   while(UCB0RXIFG & IFG2);
    •   IFG2 &= ~UCB0RXIFG;
    •   data  = UCB0RXBUF;
    •   
    •   UCB0CTL1 |= UCTXSTP;            // I2C stop condition
    •   while (UCB0CTL1 & UCTXSTP);     // Ensure stop condition got sent
    •   return data;
    • }
    • /*----------------------------------------------------------------------------*/
    • // Description:
    • //   Acknowledge Polling. The EEPROM will not acknowledge if a write cycle is
    • //   in progress. It can be used to determine when a write cycle is completed.
    • /*----------------------------------------------------------------------------*/
    • void I2C_AckPolling(void)
    • {
    •   while (UCB0STAT & UCBUSY)
    •   {     
    •     ;// wait until I2C module has
    •   }  // finished all operations
    •   do
    •   {
    •     UCB0STAT = 0x00;              // clear I2C interrupt flags
    •     UCB0CTL1 |= UCTR;             // I2CTRX=1 => Transmit Mode (R/W bit = 0)
    •     UCB0CTL1 &= ~UCTXSTT;
    •     UCB0CTL1 |= UCTXSTT;          // start condition is generated
    •     while (UCB0CTL1 & UCTXSTT)    // wait till I2CSTT bit was cleared
    •     {   
    •       if (!(UCNACKIFG & UCB0STAT))
    •       {  
    •         break;// Break out if ACK received
    •       }
    •     }
    •     UCB0CTL1 |= UCTXSTP;          // stop condition is generated after
    •                                   // slave address was sent => I2C communication is started
    •     while (UCB0CTL1 & UCTXSTP)
    •     {   
    •       ;// wait till stop bit is reset
    •     }
    •     __delay_cycles(500);          // Software delay
    •    
    •   } while (UCNACKIFG & UCB0STAT);
    • }
    • // USCI_B0 Data ISR
    • // Notice : UCSIAB0RX_ISR should be handle with UCSIAB0TX_ISR
    • #pragma vector = USCIAB0TX_VECTOR
    • __interrupt void USCIAB0TX_ISR(void)
    • {
    •   if (UCB0TXIFG & IFG2)      // TX
    •   {
    •     UCB0TXBUF = I2CSendBuffer[I2CSendPtr]; // Load TX buffer
    •     I2CSendPtr--;                          // Decrement TX byte counter
    •     if (I2CSendPtr < 0)
    •     {               
    •       while (!(IFG2 & UCB0TXIFG));         // wait for tx complete
    •       IE2 &= ~UCB0TXIE;                    // disable interrupts.
    •       IFG2 &= ~UCB0TXIFG;                  // Clear USCI_B0 TX int flag
    •       __bic_SR_register_on_exit(LPM0_bits);// Exit LPM0
    •     }
    •   }
    •   else if (UCB0RXIFG & IFG2) // RX
    •   {
    •     I2CRecvBuffer = UCB0RXBUF;             // store received data in buffer
    •     __bic_SR_register_on_exit(LPM0_bits);  // Exit LPM0
    •   }
    • }






更多回帖

0 0 0

相关问答

MPU6050 六轴传感器
发帖
电子发烧友APP
|投诉反馈|电子发烧友网
© 2021 bbs.elecfans.com
湘ICP备2023018690号
×
20
完善资料，
赚取积分