logzhan
/
GeBalanceBot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeBalanceBot/4.WHEELTEC B570 平衡小车源码（库函数精.../MiniBalance/MPU6050/MPU6050.c

693 lines
23 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

//#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----------------------------
Reference in New Issue View Git Blame Copy Permalink