mpu6050 six-axis sensor msp430 driver

灞波儿奔

/*
* 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 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(); //Time-consuming 6.15ms
//Uart1_Send_AF(); //Time-consuming 8.42ms
P1OUT ^= BIT0;
Get_Attitude();
//ReadMPU6050All() ;
//MPU6050_Dataanl(); //Without multi-byte read, it takes 6ms, with multi-byte read, it takes 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]; //Store data after I2C reads

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

/**********************************************************/
//Function name: void MPU6050_Dataanl
//Entry parameter: None
//Exit parameter: None
//Function function: Read and process MPU6050 data
/**********************************************************/
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;
//------------------------------------------------------------------//
//Compensation offset
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

}

/**************************************************************/
//Function name: void MPU6050Init
//Entry parameter: None
//Exit parameter: None
//Function function: MPU6050 initialization
/**********************************************************/
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);
}
/**************************************************************/ 8; temp+=lo; return temp ; #else return( Double_Read_ADXL345(address)); #endif } /**************************************************************/
//Function name: int Get16Bit
//Entry parameter: address: the address of the data to be read
//Exit parameter: none
//Function function: Get the data on the corresponding address of 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 } /******************************************************************/ //Function name: //Entry parameter: none //Exit parameter: none //Function function: Get the acceleration data on the corresponding axis of 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);
}
/***********************************************************/
//Function name:
//Input parameter: None
//Exit parameter: None
//Function function: Get the angular velocity data on the corresponding axis of 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
}

/**************************************
Start signal
******************************************/
void ADXL345_Start(void)
{
SET_SDA_OUT;
SDA_HIGH; //Pull up the data line
SCL_HIGH; //Pull up the clock line
Delay5us(); //Delay
SDA_LOW; //Generate a falling edge
Delay5us(); //Delay
SCL_LOW; //Pull down the clock line
}

/**************************************
Stop signal
**************************************/
void ADXL345_Stop(void)
{
SET_SDA_OUT;
SDA_LOW; //Pull down the data line
SCL_HIGH; //Pull up the clock line
Delay5us(); //Delay
SDA_HIGH; //Generate rising edge
Delay5us(); //Delay
}

/**************************************
Send response signal
Input parameter: ack (0:ACK 1:NAK)
**************************************/
void ADXL345_SendACK(unsigned char ack)
{
SET_SDA_OUT;
if(ack)
SDA_HIGH; //Write NACK response signal
else
SDA_LOW; //Write ACK response signal
SCL_HIGH; //Pull up the clock line
Delay5us(); //Delay
SCL_LOW; //Pull down the clock line
Delay5us(); //Delay
}

/**************************************
Receive response signal
**************************************/
unsigned char ADXL345_RecvACK(void)
{
unsigned char ack;
//------------------//
//The following two sentences must not be swapped in order, otherwise it will cause timing errors
SET_SDA_IN;
SCL_HIGH; //Pull up the clock line
//-----------------//
Delay5us(); //Delay
ack = SDA_IN; //Read response signal
SCL_LOW; //Pull down the clock line
Delay5us(); //Delay
return ack;
}

/**************************************
Send a byte of data to the IIC bus
**************************************/
void ADXL345_Senduchar(unsigned char dat)
{
unsigned char i,m;
SET_SDA_OUT;
for (i=8; i!=0; i--) //8-bit counter
{
m=dat & 0x80; //Shift out the highest bit of the dataif
(m == 0x80)
SDA_HIGH;
else
SDA_LOW;
SCL_HIGH; //Pull up the clock line
Delay5us(); //Delay
SCL_LOW; //Pull down the clock line
dat=dat<<1;
Delay5us(); //Delay
}
ADXL345_RecvACK();
}

/**************************************
Receive a byte of data from the IIC bus
**************************************/
unsigned char ADXL345_Recvuchar(void)
{
unsigned char i;
unsigned char dat = 0;
unsigned char m;
SDA_HIGH; //Enable internal pull-up, prepare to read data,
SET_SDA_IN;
for (i=8; i!=0; i--) //8-bit counter
{
dat <<= 1;
SCL_HIGH; //Pull up the clock line
SET_SDA_IN;
m = SDA_IN;
if(m == I2C_SDA)
dat = dat|0x01;
Delay5us(); //Delay
SCL_LOW; //Pull down the clock line
Delay5us(); //Delay
}
return dat;
}

//******Single byte write********************************************

void Single_Write_ADXL345(unsigned char REG_Address,unsigned char REG_data)
{
ADXL345_Start(); //Start signal
ADXL345_Senduchar(SlaveAddress); //Send device address + write signal
ADXL345_Senduchar(REG_Address); //Internal register address, please refer to Chinese pdf page 22
ADXL345_Senduchar(REG_data); //Internal register data, please refer to Chinese pdf page 22
ADXL345_Stop(); //Send stop signal
}

//********Single byte read*****************************************
unsigned char Single_Read_ADXL345(unsigned char REG_Address)
{ unsigned char REG_data=0;
ADXL345_Start(); //Start signal
ADXL345_Senduchar(SlaveAddress); //Send device address + write signal
ADXL345_Senduchar(REG_Address); //Send storage unit address, starting from 0
ADXL345_Start(); //Start signal
ADXL345_Senduchar(SlaveAddress+1); //Send device address + read signal
REG_data=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(1); //NACK
ADXL345_Stop(); //Stop signal
return REG_data;
}
//********Multi-byte read********************************************
int Double_Read_ADXL345(unsigned char REG_Address)
{
unsigned char ValueL=0;
int Value=0;
ADXL345_Start(); //Start signal
ADXL345_Senduchar(SlaveAddress); //Send device address + write signal
ADXL345_Senduchar(REG_Address); //Send storage unit address, starting from 0
ADXL345_Start(); //Start signal
ADXL345_Senduchar(SlaveAddress+1); //Send device address + read signal

Value=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

ValueL=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(1); //NACK

ADXL345_Stop(); //Stop signal
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(); //Start signal
ADXL345_Senduchar(SlaveAddress); //Send device address + write signal
ADXL345_Senduchar(0x3B); //Send storage unit address, starting from 0x3b
ADXL345_Start(); //Start signal
ADXL345_Senduchar(SlaveAddress+1); //Send device address + read signal

TempAcc2=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempAcc1=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempAcc4=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempAcc3=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempAcc6=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempAcc5=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

ADXL345_Recvuchar(); //Discard data of discontinuous addresses
ADXL345_SendACK(0); //ACK

ADXL345_Recvuchar(); //Discard data of discontinuous addresses
ADXL345_SendACK(0);

TempGyro2=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempGyro1=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0);

TempGyro4=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempGyro3=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0);

TempGyro6=ADXL345_Recvuchar(); //Read register data
ADXL345_SendACK(0); //ACK

TempGyro5=ADXL345_Recvuchar(); //Read register data
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

// Divide the "int" type data into two single-byte data
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

/************************************************************
* Name: USCI_A0_init
* Function: Serial port initialization
* Entry parameter: None
* Exit parameter: None
* Description: Set P1.1 and P1.2 as serial communication ports
**********************************************************/
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
}

/************************************************************
*Name: UartTX_Send_String
*Function: Serial port sends string function
*Input parameter: *data: data pointer len: data length
*Output parameter: None
*Description:
**************************************************************/
void UartTX_Send_String(unsigned char *Data,int len)
{
int j;
for(j=0;j<len;j++)
{
UartTX_Send_char(*Data++);
}
}

/************************************************************
*Name: UartTX_Send_char
*Function: Serial port sends character function
*Input parameter: c
*Exit parameter: None
*Description:
**********************************************************/
unsigned char UartTX_Send_char(unsigned char c)
{
UCA0TXBUF=c;
while(!(IFG2&UCA0TXIFG));
IFG2&=~UCA0TXIFG;
return c;
}
/*************************************************************
*Name: int putchar
*Function: Serial port sends character function
*Input parameter: ch
*Exit parameter: None
*Description:
**********************************************************/
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++;
}
}

/************************************************************
*Name: void Uart1_Send_AF
*Function: Serial port sends attitude data
*Input parameter: None
*Output parameter: None
*Description: Each execution of this function is considered as a frame of data, the frame header is 0X88, and the function word
* is 0XAF
**********************************************************/
void Uart1_Send_AF(void)
{
unsigned char sum = 0;//Accumulate the value of the data sent by the serial port and use
unsigned int _temp for verification;
sum += UartTX_Send_char(0x88); //Frame headersum
+= UartTX_Send_char(0xAF); //Function wordsum

+= UartTX_Send_char(0x1c);
sum += UartTX_Send_char( BYTE1(MPU6050_ACC_LAST_X) ); //Send the high 8 bits of the acceleration X-axis datasum
+= UartTX_Send_char( BYTE0(MPU6050_ACC_LAST_X) ); //Send the lower 8 bits of the acceleration X-axis data

sum += UartTX_Send_char( BYTE1(MPU6050_ACC_LAST_Y) ); //Send the upper 8 bits of the acceleration Y-axis data
sum += UartTX_Send_char( BYTE0(MPU6050_ACC_LAST_Y) ); //Send the lower 8 bits of the acceleration Y-axis data

sum += UartTX_Send_char( BYTE1(MPU6050_ACC_LAST_Z) ); //Send the upper 8 bits of the acceleration Z-axis data
sum += UartTX_Send_char( BYTE0(MPU6050_ACC_LAST_Z) ); //Send the lower 8 bits of the acceleration Z-axis data

sum += UartTX_Send_char( BYTE1(MPU6050_GYRO_LAST_X) ); //Send the high 8 bits of the gyroscope X-axis data
sum += UartTX_Send_char( BYTE0(MPU6050_GYRO_LAST_X) ); //Send the low 8 bits of the gyroscope X-axis data

sum += UartTX_Send_char( BYTE1(MPU6050_GYRO_LAST_Y) ); //Send the high 8 bits of the gyroscope Y-axis data
sum += UartTX_Send_char( BYTE0(MPU6050_GYRO_LAST_Y) ); //Send the low 8 bits of the gyroscope Y-axis data

sum += UartTX_Send_char( BYTE1(MPU6050_GYRO_LAST_Z) ); //Send the high 8 bits of the gyroscope Z-axis data
sum += UartTX_Send_char( BYTE0(MPU6050_GYRO_LAST_Z) ); //Send the low 8 bits of the gyroscope Z-axis data
sum +=
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( BYTE 1(_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); //Serial port sends accumulated value for verification
}

/*
* This file contains some IMU operation.
* By IC爬虫 (1394024051@qq.com)
* 2014-4-29 v1.0
*/
#include "IMU.h"

#define RtA 57.324841 //Radians to degrees
#define AtR 0.0174533 //Degrees to degrees
#define Acc_G 0.0011963 //Acceleration to G
#define Gyro_G 0.0152672 //Angular velocity to degrees
#define Gyro_Gr 0.0002663
#define FILTER_NUM 20

int ACC_AVG_X,ACC_AVG_Y,ACC_AVG_Z; //ACC after average filtering
float GYRO_I_X,GYRO_I_Y,GYRO_I_Z; //Gyro integral
float EXP_ANGLE_X,EXP_ANGLE_Y,EXP_ANGLE_Z; //Expected angle
float DIF_ANGLE_X,DIF_ANGLE_Y,DIF_ANGLE_Z; //Difference between expected angle and actual angle
float Q_ANGLE_X,Q_ANGLE_Y,Q_ANGLE_Z; //Angle calculated by quaternion

int ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM]; //Acceleration sliding window filter array

/**************************************************************/
//Function name: Prepare_Data
//Entry parameter: None
//Exit parameter: None
//Function function: Read MPU6050 data for smoothing and filtering, and prepare data for subsequent calculations
/**********************************************************/
void Prepare_Data(void)
{
static unsigned char filter_cnt=0;
long int temp1=0,temp2=0,temp3=0;
unsigned char i;

MPU6050_Dataanl(); //Complete the reading and calculation of sensor data, and simply process the data

ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST_X; //Update the sliding window array 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 is the time interval, the execution cycle of the two prepare functions
GYRO_I_Y += MPU6050_GYRO_LAST_Y*Gyro_G*0.02; //The time measured by the oscilloscope is 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
/******************************************************/
//Function name: IMUupdate
//Entry parameters: gx: floating-point gyroscope x-axis data
// gy: floating-point gyroscope y-axis data
// gz: floating-point gyroscope z-axis data
// ax: floating-point acceleration x-axis data
// ay: floating-point acceleration y-axis data
// az: floating-point acceleration z-axis data
//Exit parameters: None
//Function function: Calculate attitude using quaternion using gyroscope and accelerometer data
/**********************************************************/
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;

//First get the values we need ready
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 data normalization
ax = ax /norm;
ay = ay / norm;
az = az / norm;

// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2); // quaternion xyz representation
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) ; // vector outer product subtraction gives the difference which is the error
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;

exInt = exInt + ex * Ki; //Integrate the error
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;

// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //Compensate the error PI to the gyroscope, that is, compensate for the zero drift
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //Here, gz will drift because there is no observer to correct it, which is manifested as the integral self-increment or self-decrement

// integrate quaternion rate and normalise //Quaternion differential equations
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 send buffer and receive buffer

int I2CSendPtr=0;

/**************************************************************/
//Function name: void I2C_Init
//Entry parameter: SlaveAddr: slave device address
//Export parameter: None
//Function function: I2C initialization, 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 } /**********************************************************/ //Function name: void I2C_WriteInit //Entry parameter:

None //Exit parameter

: None //
Function function : I2C write data initialization , send mode, receive interrupt disable, send interrupt disable / ************************************************************** / 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 } /**************************************************************/ //Function name: void I2C_ReadInit //Entry parameter: None //Exit parameter: None //Function function: I2C read data initialization, receive mode, receive interrupt disable, send interrupt disable /**************************************************************/ 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 } /**********************************************************/ //Function name: I2C_Write //Entry parameter: address: the address of the device to be written to // data: the data to be sent //Exit parameter: None //Function function: I2C sends data /**************************************************************/ 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
}
}

mpu6050 six-axis sensor msp430 driver [Copy link]