693 lines
23 KiB
C
693 lines
23 KiB
C
//#include "MPU6050.h"
|
||
//#include "IOI2C.h"
|
||
//#include "usart.h"
|
||
//#define PRINT_ACCEL (0x01)
|
||
//#define PRINT_GYRO (0x02)
|
||
//#define PRINT_QUAT (0x04)
|
||
//#define ACCEL_ON (0x01)
|
||
//#define GYRO_ON (0x02)
|
||
//#define MOTION (0)
|
||
//#define NO_MOTION (1)
|
||
//#define DEFAULT_MPU_HZ (200)
|
||
//#define FLASH_SIZE (512)
|
||
//#define FLASH_MEM_START ((void*)0x1800)
|
||
//#define q30 1073741824.0f
|
||
//short gyro[3], accel[3], sensors;
|
||
//float Pitch,Roll;
|
||
//float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
|
||
//static signed char gyro_orientation[9] = {-1, 0, 0,
|
||
// 0,-1, 0,
|
||
// 0, 0, 1};
|
||
|
||
//static unsigned short inv_row_2_scale(const signed char *row)
|
||
//{
|
||
// unsigned short b;
|
||
|
||
// if (row[0] > 0)
|
||
// b = 0;
|
||
// else if (row[0] < 0)
|
||
// b = 4;
|
||
// else if (row[1] > 0)
|
||
// b = 1;
|
||
// else if (row[1] < 0)
|
||
// b = 5;
|
||
// else if (row[2] > 0)
|
||
// b = 2;
|
||
// else if (row[2] < 0)
|
||
// b = 6;
|
||
// else
|
||
// b = 7; // error
|
||
// return b;
|
||
//}
|
||
|
||
|
||
//static unsigned short inv_orientation_matrix_to_scalar(
|
||
// const signed char *mtx)
|
||
//{
|
||
// unsigned short scalar;
|
||
// scalar = inv_row_2_scale(mtx);
|
||
// scalar |= inv_row_2_scale(mtx + 3) << 3;
|
||
// scalar |= inv_row_2_scale(mtx + 6) << 6;
|
||
|
||
|
||
// return scalar;
|
||
//}
|
||
|
||
//static void run_self_test(void)
|
||
//{
|
||
// int result;
|
||
// long gyro[3], accel[3];
|
||
|
||
// result = mpu_run_self_test(gyro, accel);
|
||
// if (result == 0x7) {
|
||
// /* Test passed. We can trust the gyro data here, so let's push it down
|
||
// * to the DMP.
|
||
// */
|
||
// float sens;
|
||
// unsigned short accel_sens;
|
||
// mpu_get_gyro_sens(&sens);
|
||
// gyro[0] = (long)(gyro[0] * sens);
|
||
// gyro[1] = (long)(gyro[1] * sens);
|
||
// gyro[2] = (long)(gyro[2] * sens);
|
||
// dmp_set_gyro_bias(gyro);
|
||
// mpu_get_accel_sens(&accel_sens);
|
||
// accel[0] *= accel_sens;
|
||
// accel[1] *= accel_sens;
|
||
// accel[2] *= accel_sens;
|
||
// dmp_set_accel_bias(accel);
|
||
// printf("setting bias succesfully ......\r\n");
|
||
// }
|
||
//}
|
||
|
||
|
||
|
||
//uint8_t buffer[14];
|
||
|
||
//int16_t MPU6050_FIFO[6][11];
|
||
//int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
|
||
|
||
|
||
|
||
///**************************************************************************
|
||
//Function: The new ADC data is updated to FIFO array for filtering
|
||
//Input : ax<61><78>ay<61><79>az<61><7A>x<EFBFBD><78>y, z-axis acceleration data<74><61>gx<67><78>gy<67><79>gz<67><7A>x. Y, z-axis angular acceleration data
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD>µ<EFBFBD>ADC<44><43><EFBFBD>ݸ<EFBFBD><DDB8>µ<EFBFBD> FIFO<46><4F><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ax<61><78>ay<61><79>az<61><7A>x<EFBFBD><78>y<EFBFBD><79>z<EFBFBD><7A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>ݣ<EFBFBD>gx<67><78>gy<67><79>gz<67><7A>x<EFBFBD><78>y<EFBFBD><79>z<EFBFBD><7A><EFBFBD>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
|
||
//void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
|
||
//{
|
||
//unsigned char i ;
|
||
//int32_t sum=0;
|
||
//for(i=1;i<10;i++){ //FIFO <20><><EFBFBD><EFBFBD>
|
||
//MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
|
||
//MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
|
||
//MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
|
||
//MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
|
||
//MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
|
||
//MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
|
||
//}
|
||
//MPU6050_FIFO[0][9]=ax;//<2F><><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7>õ<EFBFBD> <20><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//MPU6050_FIFO[1][9]=ay;
|
||
//MPU6050_FIFO[2][9]=az;
|
||
//MPU6050_FIFO[3][9]=gx;
|
||
//MPU6050_FIFO[4][9]=gy;
|
||
//MPU6050_FIFO[5][9]=gz;
|
||
|
||
//sum=0;
|
||
//for(i=0;i<10;i++){ //<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ĺϣ<C4BA><CFA3><EFBFBD>ȡƽ<C8A1><C6BD>ֵ
|
||
// sum+=MPU6050_FIFO[0][i];
|
||
//}
|
||
//MPU6050_FIFO[0][10]=sum/10;
|
||
|
||
//sum=0;
|
||
//for(i=0;i<10;i++){
|
||
// sum+=MPU6050_FIFO[1][i];
|
||
//}
|
||
//MPU6050_FIFO[1][10]=sum/10;
|
||
|
||
//sum=0;
|
||
//for(i=0;i<10;i++){
|
||
// sum+=MPU6050_FIFO[2][i];
|
||
//}
|
||
//MPU6050_FIFO[2][10]=sum/10;
|
||
|
||
//sum=0;
|
||
//for(i=0;i<10;i++){
|
||
// sum+=MPU6050_FIFO[3][i];
|
||
//}
|
||
//MPU6050_FIFO[3][10]=sum/10;
|
||
|
||
//sum=0;
|
||
//for(i=0;i<10;i++){
|
||
// sum+=MPU6050_FIFO[4][i];
|
||
//}
|
||
//MPU6050_FIFO[4][10]=sum/10;
|
||
|
||
//sum=0;
|
||
//for(i=0;i<10;i++){
|
||
// sum+=MPU6050_FIFO[5][i];
|
||
//}
|
||
//MPU6050_FIFO[5][10]=sum/10;
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Setting the clock source of mpu6050
|
||
//Input : source<63><65>Clock source number
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20><>ʱ<EFBFBD><CAB1>Դ
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>source<63><65>ʱ<EFBFBD><CAB1>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
// * CLK_SEL | Clock Source
|
||
// * --------+--------------------------------------
|
||
// * 0 | Internal oscillator
|
||
// * 1 | PLL with X Gyro reference
|
||
// * 2 | PLL with Y Gyro reference
|
||
// * 3 | PLL with Z Gyro reference
|
||
// * 4 | PLL with external 32.768kHz reference
|
||
// * 5 | PLL with external 19.2MHz reference
|
||
// * 6 | Reserved
|
||
// * 7 | Stops the clock and keeps the timing generator in reset
|
||
//**************************************************************************/
|
||
//void MPU6050_setClockSource(uint8_t source){
|
||
// IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
|
||
|
||
//}
|
||
|
||
///** Set full-scale gyroscope range.
|
||
// * @param range New full-scale gyroscope range value
|
||
// * @see getFullScaleRange()
|
||
// * @see MPU6050_GYRO_FS_250
|
||
// * @see MPU6050_RA_GYRO_CONFIG
|
||
// * @see MPU6050_GCONFIG_FS_SEL_BIT
|
||
// * @see MPU6050_GCONFIG_FS_SEL_LENGTH
|
||
// */
|
||
//void MPU6050_setFullScaleGyroRange(uint8_t range) {
|
||
// IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Setting the maximum range of mpu6050 accelerometer
|
||
//Input : range<67><65>Acceleration maximum range number
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20><><EFBFBD>ٶȼƵ<C8BC><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>range<67><65><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̱<EFBFBD><CCB1><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
////#define MPU6050_ACCEL_FS_2 0x00 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-2G
|
||
////#define MPU6050_ACCEL_FS_4 0x01 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-4G
|
||
////#define MPU6050_ACCEL_FS_8 0x02 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-8G
|
||
////#define MPU6050_ACCEL_FS_16 0x03 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-16G
|
||
//void MPU6050_setFullScaleAccelRange(uint8_t range) {
|
||
// IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Set mpu6050 to sleep mode or not
|
||
//Input : enable<6C><65>1<EFBFBD><31>sleep<65><70>0<EFBFBD><30>work<72><6B>
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>˯<EFBFBD><CBAF>ģʽ
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>enable<6C><65>1<EFBFBD><31>˯<EFBFBD><CBAF><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//void MPU6050_setSleepEnabled(uint8_t enabled) {
|
||
// IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Read identity
|
||
//Input : none
|
||
//Output : 0x68
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ȡ MPU6050 WHO_AM_I <20><>ʶ
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5>0x68
|
||
//**************************************************************************/
|
||
//uint8_t MPU6050_getDeviceID(void) {
|
||
|
||
// IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
|
||
// return buffer[0];
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Check whether mpu6050 is connected
|
||
//Input : none
|
||
//Output : 1<><31>Connected<65><64>0<EFBFBD><30>Not connected
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>MPU6050 <20>Ƿ<EFBFBD><C7B7>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӣ<EFBFBD>0<EFBFBD><30>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//uint8_t MPU6050_testConnection(void) {
|
||
// if(MPU6050_getDeviceID() == 0x68) //0b01101000;
|
||
// return 1;
|
||
// else return 0;
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Setting whether mpu6050 is the host of aux I2C cable
|
||
//Input : enable<6C><65>1<EFBFBD><31>yes<65><73>0;not
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD>ΪAUX I2C<32>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>enable<6C><65>1<EFBFBD><31><EFBFBD>ǣ<EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
|
||
// IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Setting whether mpu6050 is the host of aux I2C cable
|
||
//Input : enable<6C><65>1<EFBFBD><31>yes<65><73>0;not
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD>ΪAUX I2C<32>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>enable<6C><65>1<EFBFBD><31><EFBFBD>ǣ<EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
|
||
// IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: initialization Mpu6050 to enter the available state
|
||
//Input : none
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ʼ<EFBFBD><CABC> MPU6050 <20>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//void MPU6050_initialize(void) {
|
||
// MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
|
||
// MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //<2F><><EFBFBD>ٶȶ<D9B6><C8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> +-2G
|
||
// MPU6050_setSleepEnabled(0); //<2F><><EFBFBD>빤<EFBFBD><EBB9A4>״̬
|
||
// MPU6050_setI2CMasterModeEnabled(0); //<2F><><EFBFBD><EFBFBD>MPU6050 <20><><EFBFBD><EFBFBD>AUXI2C
|
||
// MPU6050_setI2CBypassEnabled(0); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>I2C<32><43> MPU6050<35><30>AUXI2C ֱͨ<D6B1>ر<EFBFBD>
|
||
//}
|
||
|
||
///**************************************************************************
|
||
//Function: Initialization of DMP in mpu6050
|
||
//Input : none
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>MPU6050<35><30><EFBFBD><EFBFBD>DMP<4D>ij<EFBFBD>ʼ<EFBFBD><CABC>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//void DMP_Init(void)
|
||
//{
|
||
// u8 temp[1]={0};
|
||
// i2cRead(0x68,0x75,1,temp);
|
||
// printf("mpu_set_sensor complete ......\r\n");
|
||
// if(temp[0]!=0x68)NVIC_SystemReset();
|
||
// if(!mpu_init())
|
||
// {
|
||
// if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
|
||
// printf("mpu_set_sensor complete ......\r\n");
|
||
// if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
|
||
// printf("mpu_configure_fifo complete ......\r\n");
|
||
// if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
|
||
// printf("mpu_set_sample_rate complete ......\r\n");
|
||
// if(!dmp_load_motion_driver_firmware())
|
||
// printf("dmp_load_motion_driver_firmware complete ......\r\n");
|
||
// if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
|
||
// printf("dmp_set_orientation complete ......\r\n");
|
||
// if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
|
||
// DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
|
||
// DMP_FEATURE_GYRO_CAL))
|
||
// printf("dmp_enable_feature complete ......\r\n");
|
||
// if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
|
||
// printf("dmp_set_fifo_rate complete ......\r\n");
|
||
// run_self_test();
|
||
// if(!mpu_set_dmp_state(1))
|
||
// printf("mpu_set_dmp_state complete ......\r\n");
|
||
// }
|
||
//}
|
||
///**************************************************************************
|
||
//Function: Read the attitude information of DMP in mpu6050
|
||
//Input : none
|
||
//Output : none
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ȡMPU6050<35><30><EFBFBD><EFBFBD>DMP<4D><50><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ϣ
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
//**************************************************************************/
|
||
//void Read_DMP(void)
|
||
//{
|
||
// unsigned long sensor_timestamp;
|
||
// unsigned char more;
|
||
// long quat[4];
|
||
|
||
// dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); //<2F><>ȡDMP<4D><50><EFBFBD><EFBFBD>
|
||
// if (sensors & INV_WXYZ_QUAT )
|
||
// {
|
||
// q0=quat[0] / q30;
|
||
// q1=quat[1] / q30;
|
||
// q2=quat[2] / q30;
|
||
// q3=quat[3] / q30; //<2F><>Ԫ<EFBFBD><D4AA>
|
||
// Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// }
|
||
|
||
//}
|
||
///**************************************************************************
|
||
//Function: Read mpu6050 built-in temperature sensor data
|
||
//Input : none
|
||
//Output : Centigrade temperature
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ȡMPU6050<35><30><EFBFBD><EFBFBD><EFBFBD>¶ȴ<C2B6><C8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//<2F><><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>
|
||
//**************************************************************************/
|
||
//int Read_Temperature(void)
|
||
//{
|
||
// float Temp;
|
||
// Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
|
||
// if(Temp>32768) Temp-=65536;
|
||
// Temp=(36.53+Temp/340)*10; //<2F>¶ȷŴ<C8B7>ʮ<EFBFBD><CAAE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// return (int)Temp;
|
||
//}
|
||
////------------------End of File----------------------------
|
||
#include "MPU6050.h"
|
||
#include "IOI2C.h"
|
||
#include "usart.h"
|
||
#define PRINT_ACCEL (0x01)
|
||
#define PRINT_GYRO (0x02)
|
||
#define PRINT_QUAT (0x04)
|
||
#define ACCEL_ON (0x01)
|
||
#define GYRO_ON (0x02)
|
||
#define MOTION (0)
|
||
#define NO_MOTION (1)
|
||
#define DEFAULT_MPU_HZ (200)
|
||
#define FLASH_SIZE (512)
|
||
#define FLASH_MEM_START ((void*)0x1800)
|
||
#define q30 1073741824.0f
|
||
short gyro[3], accel[3], sensors;
|
||
float Pitch,Roll;
|
||
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
|
||
static signed char gyro_orientation[9] = {-1, 0, 0,
|
||
0,-1, 0,
|
||
0, 0, 1};
|
||
|
||
static unsigned short inv_row_2_scale(const signed char *row)
|
||
{
|
||
unsigned short b;
|
||
|
||
if (row[0] > 0)
|
||
b = 0;
|
||
else if (row[0] < 0)
|
||
b = 4;
|
||
else if (row[1] > 0)
|
||
b = 1;
|
||
else if (row[1] < 0)
|
||
b = 5;
|
||
else if (row[2] > 0)
|
||
b = 2;
|
||
else if (row[2] < 0)
|
||
b = 6;
|
||
else
|
||
b = 7; // error
|
||
return b;
|
||
}
|
||
|
||
|
||
static unsigned short inv_orientation_matrix_to_scalar(
|
||
const signed char *mtx)
|
||
{
|
||
unsigned short scalar;
|
||
scalar = inv_row_2_scale(mtx);
|
||
scalar |= inv_row_2_scale(mtx + 3) << 3;
|
||
scalar |= inv_row_2_scale(mtx + 6) << 6;
|
||
|
||
|
||
return scalar;
|
||
}
|
||
|
||
static void run_self_test(void)
|
||
{
|
||
int result;
|
||
long gyro[3], accel[3];
|
||
|
||
result = mpu_run_self_test(gyro, accel);
|
||
if (result == 0x7) {
|
||
/* Test passed. We can trust the gyro data here, so let's push it down
|
||
* to the DMP.
|
||
*/
|
||
float sens;
|
||
unsigned short accel_sens;
|
||
mpu_get_gyro_sens(&sens);
|
||
gyro[0] = (long)(gyro[0] * sens);
|
||
gyro[1] = (long)(gyro[1] * sens);
|
||
gyro[2] = (long)(gyro[2] * sens);
|
||
dmp_set_gyro_bias(gyro);
|
||
mpu_get_accel_sens(&accel_sens);
|
||
accel[0] *= accel_sens;
|
||
accel[1] *= accel_sens;
|
||
accel[2] *= accel_sens;
|
||
dmp_set_accel_bias(accel);
|
||
printf("setting bias succesfully ......\r\n");
|
||
}
|
||
}
|
||
|
||
|
||
|
||
uint8_t buffer[14];
|
||
|
||
int16_t MPU6050_FIFO[6][11];
|
||
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
|
||
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD>µ<EFBFBD>ADC<44><43><EFBFBD>ݸ<EFBFBD><DDB8>µ<EFBFBD> FIFO<46><4F><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
|
||
*******************************************************************************/
|
||
|
||
void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
|
||
{
|
||
unsigned char i ;
|
||
int32_t sum=0;
|
||
for(i=1;i<10;i++){ //FIFO <20><><EFBFBD><EFBFBD>
|
||
MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
|
||
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
|
||
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
|
||
MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
|
||
MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
|
||
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
|
||
}
|
||
MPU6050_FIFO[0][9]=ax;//<2F><><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7>õ<EFBFBD> <20><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
MPU6050_FIFO[1][9]=ay;
|
||
MPU6050_FIFO[2][9]=az;
|
||
MPU6050_FIFO[3][9]=gx;
|
||
MPU6050_FIFO[4][9]=gy;
|
||
MPU6050_FIFO[5][9]=gz;
|
||
|
||
sum=0;
|
||
for(i=0;i<10;i++){ //<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ĺϣ<C4BA><CFA3><EFBFBD>ȡƽ<C8A1><C6BD>ֵ
|
||
sum+=MPU6050_FIFO[0][i];
|
||
}
|
||
MPU6050_FIFO[0][10]=sum/10;
|
||
|
||
sum=0;
|
||
for(i=0;i<10;i++){
|
||
sum+=MPU6050_FIFO[1][i];
|
||
}
|
||
MPU6050_FIFO[1][10]=sum/10;
|
||
|
||
sum=0;
|
||
for(i=0;i<10;i++){
|
||
sum+=MPU6050_FIFO[2][i];
|
||
}
|
||
MPU6050_FIFO[2][10]=sum/10;
|
||
|
||
sum=0;
|
||
for(i=0;i<10;i++){
|
||
sum+=MPU6050_FIFO[3][i];
|
||
}
|
||
MPU6050_FIFO[3][10]=sum/10;
|
||
|
||
sum=0;
|
||
for(i=0;i<10;i++){
|
||
sum+=MPU6050_FIFO[4][i];
|
||
}
|
||
MPU6050_FIFO[4][10]=sum/10;
|
||
|
||
sum=0;
|
||
for(i=0;i<10;i++){
|
||
sum+=MPU6050_FIFO[5][i];
|
||
}
|
||
MPU6050_FIFO[5][10]=sum/10;
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_setClockSource(uint8_t source)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD> MPU6050 <20><>ʱ<EFBFBD><CAB1>Դ
|
||
* CLK_SEL | Clock Source
|
||
* --------+--------------------------------------
|
||
* 0 | Internal oscillator
|
||
* 1 | PLL with X Gyro reference
|
||
* 2 | PLL with Y Gyro reference
|
||
* 3 | PLL with Z Gyro reference
|
||
* 4 | PLL with external 32.768kHz reference
|
||
* 5 | PLL with external 19.2MHz reference
|
||
* 6 | Reserved
|
||
* 7 | Stops the clock and keeps the timing generator in reset
|
||
*******************************************************************************/
|
||
void MPU6050_setClockSource(uint8_t source){
|
||
IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
|
||
|
||
}
|
||
|
||
/** Set full-scale gyroscope range.
|
||
* @param range New full-scale gyroscope range value
|
||
* @see getFullScaleRange()
|
||
* @see MPU6050_GYRO_FS_250
|
||
* @see MPU6050_RA_GYRO_CONFIG
|
||
* @see MPU6050_GCONFIG_FS_SEL_BIT
|
||
* @see MPU6050_GCONFIG_FS_SEL_LENGTH
|
||
*/
|
||
void MPU6050_setFullScaleGyroRange(uint8_t range) {
|
||
IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_setFullScaleAccelRange(uint8_t range)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD> MPU6050 <20><><EFBFBD>ٶȼƵ<C8BC><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
*******************************************************************************/
|
||
void MPU6050_setFullScaleAccelRange(uint8_t range) {
|
||
IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_setSleepEnabled(uint8_t enabled)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>˯<EFBFBD><CBAF>ģʽ
|
||
enabled =1 ˯<><CBAF>
|
||
enabled =0 <20><><EFBFBD><EFBFBD>
|
||
*******************************************************************************/
|
||
void MPU6050_setSleepEnabled(uint8_t enabled) {
|
||
IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: uint8_t MPU6050_getDeviceID(void)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><>ȡ MPU6050 WHO_AM_I <20><>ʶ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0x68
|
||
*******************************************************************************/
|
||
uint8_t MPU6050_getDeviceID(void) {
|
||
|
||
IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
|
||
return buffer[0];
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: uint8_t MPU6050_testConnection(void)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD>MPU6050 <20>Ƿ<EFBFBD><C7B7>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
|
||
*******************************************************************************/
|
||
uint8_t MPU6050_testConnection(void) {
|
||
if(MPU6050_getDeviceID() == 0x68) //0b01101000;
|
||
return 1;
|
||
else return 0;
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD>ΪAUX I2C<32>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>
|
||
*******************************************************************************/
|
||
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
|
||
IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_setI2CBypassEnabled(uint8_t enabled)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD>ΪAUX I2C<32>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>
|
||
*******************************************************************************/
|
||
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
|
||
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
|
||
}
|
||
|
||
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
|
||
*<2A><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>: void MPU6050_initialize(void)
|
||
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><>ʼ<EFBFBD><CABC> MPU6050 <20>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
|
||
*******************************************************************************/
|
||
void MPU6050_initialize(void) {
|
||
MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
|
||
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //<2F><><EFBFBD>ٶȶ<D9B6><C8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> +-2G
|
||
MPU6050_setSleepEnabled(0); //<2F><><EFBFBD>빤<EFBFBD><EBB9A4>״̬
|
||
MPU6050_setI2CMasterModeEnabled(0); //<2F><><EFBFBD><EFBFBD>MPU6050 <20><><EFBFBD><EFBFBD>AUXI2C
|
||
MPU6050_setI2CBypassEnabled(0); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>I2C<32><43> MPU6050<35><30>AUXI2C ֱͨ<D6B1>ر<EFBFBD>
|
||
}
|
||
|
||
/**************************************************************************
|
||
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>MPU6050<EFBFBD><EFBFBD><EFBFBD><EFBFBD>DMP<EFBFBD>ij<EFBFBD>ʼ<EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD> <20>ߣ<EFBFBD>ƽ<EFBFBD><C6BD>С<EFBFBD><D0A1>֮<EFBFBD><D6AE>
|
||
**************************************************************************/
|
||
void DMP_Init(void)
|
||
{
|
||
u8 temp[1]={0};
|
||
i2cRead(0x68,0x75,1,temp);
|
||
Flag_Show=1;
|
||
printf("mpu_set_sensor complete ......\r\n");
|
||
if(temp[0]!=0x68)NVIC_SystemReset();
|
||
if(!mpu_init())
|
||
{
|
||
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
|
||
printf("mpu_set_sensor complete ......\r\n");
|
||
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
|
||
printf("mpu_configure_fifo complete ......\r\n");
|
||
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
|
||
printf("mpu_set_sample_rate complete ......\r\n");
|
||
if(!dmp_load_motion_driver_firmware())
|
||
printf("dmp_load_motion_driver_firmware complete ......\r\n");
|
||
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
|
||
printf("dmp_set_orientation complete ......\r\n");
|
||
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
|
||
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
|
||
DMP_FEATURE_GYRO_CAL))
|
||
printf("dmp_enable_feature complete ......\r\n");
|
||
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
|
||
printf("dmp_set_fifo_rate complete ......\r\n");
|
||
run_self_test();
|
||
if(!mpu_set_dmp_state(1))
|
||
printf("mpu_set_dmp_state complete ......\r\n");
|
||
}
|
||
Flag_Show=0;
|
||
|
||
}
|
||
/**************************************************************************
|
||
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡMPU6050<EFBFBD><EFBFBD><EFBFBD><EFBFBD>DMP<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ϣ
|
||
<EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD> <20>ߣ<EFBFBD>ƽ<EFBFBD><C6BD>С<EFBFBD><D0A1>֮<EFBFBD><D6AE>
|
||
**************************************************************************/
|
||
void Read_DMP(void)
|
||
{
|
||
unsigned long sensor_timestamp;
|
||
unsigned char more;
|
||
long quat[4];
|
||
|
||
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
|
||
if (sensors & INV_WXYZ_QUAT )
|
||
{
|
||
q0=quat[0] / q30;
|
||
q1=quat[1] / q30;
|
||
q2=quat[2] / q30;
|
||
q3=quat[3] / q30;
|
||
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
|
||
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
|
||
}
|
||
|
||
}
|
||
/**************************************************************************
|
||
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡMPU6050<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶ȴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>
|
||
<EFBFBD><EFBFBD> <20>ߣ<EFBFBD>ƽ<EFBFBD><C6BD>С<EFBFBD><D0A1>֮<EFBFBD><D6AE>
|
||
**************************************************************************/
|
||
int Read_Temperature(void)
|
||
{
|
||
float Temp;
|
||
Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
|
||
if(Temp>32768) Temp-=65536;
|
||
Temp=(36.53+Temp/340)*10;
|
||
return (int)Temp;
|
||
}
|
||
//------------------End of File----------------------------
|