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 Normal View History

更新初版工程
2024-01-14 19:28:00 +08:00
//#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>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>ADC<EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><EFBFBD>µ<EFBFBD> FIFO<EFBFBD><EFBFBD><EFBFBD>飬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><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>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_setClockSource(uint8_t source)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD><EFBFBD>ʱ<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);
}
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_setFullScaleAccelRange(uint8_t range)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD><EFBFBD><EFBFBD>ٶȼƵ<EFBFBD><EFBFBD><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>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_setSleepEnabled(uint8_t enabled)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˯<EFBFBD><EFBFBD>ģʽ
enabled =1 ˯<EFBFBD><EFBFBD>
enabled =0 <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: uint8_t MPU6050_getDeviceID(void)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>ȡ MPU6050 WHO_AM_I <EFBFBD><EFBFBD>ʶ <EFBFBD><EFBFBD><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>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: uint8_t MPU6050_testConnection(void)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPU6050 <EFBFBD>Ƿ<EFBFBD><EFBFBD>Ѿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {
if(MPU6050_getDeviceID() == 0x68) //0b01101000;
return 1;
else return 0;
}
/**************************ʵ<>ֺ<EFBFBD><D6BA><EFBFBD>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD>Ƿ<EFBFBD>ΪAUX I2C<EFBFBD>ߵ<EFBFBD><EFBFBD><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>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD>Ƿ<EFBFBD>ΪAUX I2C<EFBFBD>ߵ<EFBFBD><EFBFBD><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>********************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD>: void MPU6050_initialize(void)
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD> MPU6050 <EFBFBD>Խ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<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>
}
/**************************************************************************
<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> ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD> <EFBFBD>ߣ<EFBFBD>ƽ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD>
**************************************************************************/
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> ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD> <EFBFBD>ߣ<EFBFBD>ƽ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>֮<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);
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> ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD> <EFBFBD>ߣ<EFBFBD>ƽ<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;
return (int)Temp;
}
//------------------End of File----------------------------