mpu6050六轴传感器msp430驱动程序
玩智能车的可以参考
#include <msp430g2553.h>
#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: ~<BIT> indicates that <BIT> 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 timerBCSplus_graceInit(); //MCLK = 16M,SMCLK = 8M,ack =32.768kWDTCTL = WDTPW | WDTTMSEL | WDTIS0;IE1 |= WDTIE;USCI_A0_init(); //uart 38400bps P1DIR |= BIT0; //initialize led control PIN _EINT();MPU6050_Init(); //initialize mpu6050while(1){/*Get_Attitude();//MPU6050_Dataanl();// ReadMPU6050All();//Prepare_Data(); //耗时6.15ms//Uart1_Send_AF(); //耗时8.42msP1OUT ^= 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 READALLMPU6050_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 IMITATEIICInitImitateIICPort();
#elseI2C_Init(SlaveAddr);
#endifI2C_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 MULTIREADunsigned char ho,lo;int temp ;ho = I2C_Read(address);lo = I2C_Read(address+1);temp=ho;temp<<=8;temp+=lo;return temp ;
#elsereturn( 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应答信号elseSDA_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;elseSDA_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); //NACKADXL345_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); //ACKValueL=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(1); //NACKADXL345_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); //ACKTempAcc1=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempAcc4=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempAcc3=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempAcc6=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempAcc5=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKADXL345_Recvuchar(); //丢弃不连续地址的数据ADXL345_SendACK(0); //ACKADXL345_Recvuchar(); //丢弃不连续地址的数据ADXL345_SendACK(0); TempGyro2=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempGyro1=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); TempGyro4=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempGyro3=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); TempGyro6=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(0); //ACKTempGyro5=ADXL345_Recvuchar(); //读出寄存器数据ADXL345_SendACK(1); //NACKADXL345_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=TXDP1SEL2 = BIT1 + BIT2; UCA0CTL1 |= UCSSEL_2; // SMCLK/*UCA0BR0 = 0x45; // 8MHz 115200UCA0BR1 = 0; // 8MHz 115200UCA0MCTL = 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<len;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 20int 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<FILTER_NUM;i++){temp1 += ACC_X_BUF[i];temp2 += ACC_Y_BUF[i];temp3 += ACC_Z_BUF[i];}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 sensorsex = (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 measurementsgx = 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 quaternionnorm = 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; // yawQ_ANGLE_Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitchQ_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_B0P1SEL2|= BIT6+BIT7; // Assign I2C pins to USCI_B0UCB0CTL1 |= UCSWRST; // Enable SW resetUCB0CTL0 = UCMST+UCMODE_3+UCSYNC; // I2C Master, synchronous modeUCB0CTL0 &= ~(UCSLA10+UCA10); //7 bit add of slave and master UCB0CTL1 = UCSSEL_2+UCSWRST; // Use SMCLK, keep SW resetUCB0BR0 = 80; // fSCL = SMCLK/12 = ~100kHzUCB0BR1 = 0;UCB0I2COA = 0x01A5; //set own address UCB0I2CSA = SlaveAddr; // Set slave addressIE2 &= ~(UCB0RXIE+UCB0TXIE); // disenable TX&RX interruptUCB0CTL1 &= ~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 interruptIE2 &= ~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 interruptIE2 &= ~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 bitwhile(!(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 flagUCB0TXBUF=data; //send data while(!(IFG2 & UCB0TXIFG )); //wait sending over IFG2 &= ~UCB0TXIFG; // Clear USCI_B0 TX int flagUCB0CTL1 |= UCTXSTP; // I2C stop conditionwhile (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 operationsI2C_WriteInit();UCB0CTL1 |= UCTXSTT; // start condition generationwhile(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 conditionwhile (UCB0CTL1 & UCTXSTP); // Ensure stop condition got sentreturn 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 operationsdo {UCB0STAT = 0x00; // clear I2C interrupt flagsUCB0CTL1 |= UCTR; // I2CTRX=1 => Transmit Mode (R/W bit = 0)UCB0CTL1 &= ~UCTXSTT;UCB0CTL1 |= UCTXSTT; // start condition is generatedwhile (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 startedwhile (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 bufferI2CSendPtr--; // Decrement TX byte counterif (I2CSendPtr < 0) { while (!(IFG2 & UCB0TXIFG)); // wait for tx completeIE2 &= ~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}
}
mpu6050六轴传感器msp430驱动程序相关推荐
- Micro Python———MPU6050六轴传感器
目录 1.什么是MPU6050? MPU6050介绍: MPU6050寄存器介绍: 2.例程 1.平台 2.目的 3.讲解 1.查阅原理图 2.流程分析 3.代码讲解 3.结果 1.什么是MPU605 ...
- 正点原子 第65讲 MPU6050 六轴传感器实验
1.MPU6050 简介 2 硬件 连接 3 源码讲解& 例程测试 上图中2 单位表示的是 度/每秒 INT 为中断输出脚 CS 为片选脚 AD0/SD0 用于设置地址 AD ...
- 《手把手教你学STM32》—MPU6050六轴传感器实验
预备知识(基于STM32开发板(正点原子)) 1.MPU6050简介 1.1.MPU6050简介-什么是MPU6050? MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件, ...
- i.MX6ULL终结者MPU6050 六轴传感器例程原理分析
MPU6050的原理部分,如图 1所示: 图 1 从原理我们可以看到MPU6050的电源使用3.3V,和i.MX6ULL通过I2C2接口,地址选择引脚AD0接到了GND上.
- 如何利用小熊派获取MPU6050六轴原始数据
摘要:使用小熊派开发板,通过硬件IIC与MPU6050六轴传感器模块通信,完成相应寄存器配置,成功获取陀螺仪.加速度计数据. 本问主要讲述使用小熊派开发板+MPU6050六轴传感器,获取加速度计以及陀 ...
- 六轴传感器基础知识学习:MPU6050特性,四元数,姿态解算,卡尔曼滤波
实际上,只要说到多少轴的传感器一般是就是指加速度传感器(即加速计).角速度传感器(即陀螺仪).磁感应传感器(即电子罗盘).这三类传感器测量的数据在空间坐标系中都可以被分解为X,Y,Z三个方向轴的力,因 ...
- STM32学习值传感器篇——MPU6050六轴加速度传感器
这个传感器整体来说不难只要将模拟iic的时序写对基本问题不大 剩下的驱动文件 按照我介绍的加进去就可以实现了,还是有官方算法的好啊,直接用就完事了,很方便,节省了不少代码开发时间, 注意mpu6050 ...
- keil4怎么移植其他人的程序_【调试笔记】韦东山:在100ask_imx6ull上移植使用六轴传感器ICM20608...
之前发了LCD调试笔记,大家很感兴趣,所以这次再来一篇:六轴传感器ICM20608驱动移植笔记,大家还需要什么移植笔记?可以留言.我们尽量满足. 1.1 移植思路 先找到驱动:也许内核里已经有,也许需 ...
- 基于战舰V3的MPU6050六轴陀螺仪实验
基于战舰V3的MPU6050六轴陀螺仪实验 陀螺仪的分类 3轴传感器指的是3轴的加速度,根据这个加速度我们解算出XY两轴的角度. 6轴传感器指的是3轴的加速度和3轴角速度,根据这两个数据我们解算出XY ...
最新文章
- 统计语言模型(Statistical Language Model)
- 尝鲜体验 VS Code Python 原生 Jupyter Notebook 支持
- 解决点击MDI父窗体下拉菜单,子窗体重复出现的问题
- Linux下生成动态链接库是否必须使用 -fPIC 的问题
- MATLAB使用教程(4)——悄悄滴上手项目
- 关于if 判断等于0 和‘0’的问题
- 一加Ace外观设计理念揭晓:主推硬朗直线条力量感/速度感十足
- 原型设计(顶会热词统计)
- 两种语言实现设计模式(C++和Java)(二:单例模式)
- php分页代码简单实现原理,php分页类之PHP分页原理+代码实现
- 习题4-6 水仙花数(20 分)
- 3D 旋转相册 立方体的盒子 相册(源码分享)
- 将jpg格式转成PDF格式的转换器
- 结构体初始化、结构体指针、结构体数组
- 动态赋值_【东莞CBD】最新动态!整合大地块!打造未来东莞第一高楼!高度按500米赋值...
- 用天文方法计算二十四节气
- poj 1655 树的重心
- 备选统驭科目的使用与配置
- 边缘计算:一文理解云边端协同架构中的高性能云计算、边缘计算、云边协同
- torch.utils.data.DataLoader()的使用