[问答]

mpu6050六轴传感器msp430驱动程序

问答对人有帮助，内容完整，我也想知道答案 0 #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_X100); sum += UartTX_Send_char( BYTE1(_temp) ); sum += UartTX_Send_char( BYTE0(_temp) ); _temp = (long int)(Q_ANGLE_Y100); 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_XGyro_G0.02;//0.0001是时间间隔，两次prepare函数的执行周期 GYRO_I_Y += MPU6050_GYRO_LAST_YGyro_G0.02;//示波器测量的得到的时间是20ms. GYRO_I_Z += MPU6050_GYRO_LAST_ZGyro_G0.02; } void Get_Attitude(void) { IMUupdate( MPU6050_GYRO_LAST_XGyro_Gr,* MPU6050_GYRO_LAST_YGyro_Gr,* MPU6050_GYRO_LAST_ZGyro_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 = q0q0;* float q0q1 = q0q1;* float q0q2 = q0q2;* // float q0q3 = q0q3;* float q1q1 = q1q1;* // float q1q2 = q1q2;* float q1q3 = q1q3;* float q2q2 = q2q2;* float q2q3 = q2q3;* float q3q3 = q3q3;* if(axayaz==0) return; norm = sqrt(axax + ayay + azaz);//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 = (ayvz - azvy) ; //向量外积在相减得到差分就是误差 ey = (azvx - axvz) ; ez = (axvy - ayvx) ; exInt = exInt + ex Ki; //对误差进行积分* eyInt = eyInt + ey Ki;* ezInt = ezInt + ez Ki;* // adjusted gyroscope measurements gx = gx + Kpex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移* gy = gy + Kpey + eyInt;* gz = gz + Kpez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，变现出来的就是积分自增或者自减* // integrate quaternion rate and normalise //四元数的微分方程 q0 = q0 + (-q1gx - q2gy - q3gz)halfT; q1 = q1 + (q0gx + q2gz - q3gy)halfT; q2 = q2 + (q0gy - q1gz + q3gx)halfT; q3 = q3 + (q0gz + q1gy - q2gx)halfT; // normalise quaternion norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; //Q_ANGLE.Yaw = atan2(2 q1 * q2 + 2 * q0 * q3, -2 * q2q2 - 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
2018-6-26 06:30:10　　评论淘帖0 邀请回答您可以邀请以下用户，快速回答问题 × 新星之火12138 该类别下有 58 个回答。邀请回答 heks 该类别下有 58 个回答。邀请回答普罗旺斯的薰衣草该类别下有 55 个回答。邀请回答 lksas 该类别下有 47 个回答。邀请回答 wang21cj 该类别下有 45 个回答。邀请回答 chm5 该类别下有 45 个回答。邀请回答 hgimtk 该类别下有 44 个回答。邀请回答 fdhsfagd 该类别下有 42 个回答。邀请回答 huzp_bbs 该类别下有 41 个回答。邀请回答 ggfx 该类别下有 41 个回答。邀请回答颜刚YanG 该类别下有 40 个回答。邀请回答 zhhx1985 该类别下有 40 个回答。邀请回答站着幻想该类别下有 40 个回答。邀请回答 K_Ming 该类别下有 39 个回答。邀请回答潘涛12345 该类别下有 39 个回答。邀请回答 kghfh 该类别下有 39 个回答。邀请回答风动影随该类别下有 39 个回答。邀请回答 zhouxk 该类别下有 39 个回答。邀请回答 kingnet6688 该类别下有 38 个回答。邀请回答 lifei639156 该类别下有 38 个回答，其中被选为最佳答案 1 次。邀请回答举报马超相关推荐 • 请问谁有mpu6050姿态传感器的底层驱动程序？ 1177 • MPU6050和匿名四轴软件通讯没有反应该怎么办？ 2800 • 关于MPU6050的加速度传感器和角速度传感器 27190 • MPU6050可以测线加速度吗？----基于Mems传感器的运动目标姿态识别系统（硬件部分） 9527 • 如何对基于stm32的mpu6050传感器进行测试呢 902 • MPU6050芯片的特点有哪些 2456 • 请问怎么校准MPU6050轴间耦合？ 2343 • 请问mpu6050 I2C接口怎么连接HMC5883L和MS5611？ 3720 • 如何使用MPU6050陀螺仪自制Arduino数字量角器？ 1643 • 请问MPU6050自检到底好吗？ 4922