cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
origincar_controller/HARDWARE/MPU6050/MPU6050.c
cyy_mac 93ca37e36c 完善阿克曼控制与高速串口遥测 
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
2026-08-12 18:51:48 +08:00

550 lines
22 KiB
C
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#include "MPU6050.h"
#include "I2C.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;
//<2F><><EFBFBD>Ư<EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD>
int Deviation_Count;
short sum_gyro[3];
short sum_accel[3];
// Gyro static error, raw data
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><C7BE>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
short Deviation_gyro[3],Original_gyro[3];
short Deviation_accel[3],Original_accel[3];
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;
//}
void MPU6050_task(void *pvParameters)
{
u32 lastWakeTime = getSysTickCnt();
while(1)
{
//This task runs at 100Hz
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>100Hz<48><7A>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));
//Read the gyroscope zero before starting
//<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(Deviation_Count<CONTROL_DELAY)
{
Deviation_Count++;
sum_gyro[0]+=gyro[0];
sum_gyro[1]+=gyro[1];
sum_gyro[2]+=gyro[2];
sum_accel[0]+=accel[0];
sum_accel[1]+=accel[1];
sum_accel[2]+=accel[2];
for(int i=0;i<3;i++)
{
Deviation_gyro[i] = (short)(sum_gyro[i] / (short)Deviation_Count);
}
// memcpy(Deviation_gyro,gyro,sizeof(gyro));
memcpy(Deviation_accel,accel,sizeof(accel));
}
//Stamp just before the I2C burst read, as close to the physical IMU
//sample as we can get without a data-ready interrupt.
//<2F><> I2C <20><>ȡǰ<C8A1><C7B0>㣬<EFBFBD>ھ<EFBFBD><DABE><EFBFBD> DRDY <20>жϵ<D0B6><CFB5><EFBFBD><EFBFBD><EFBFBD>¾<EFBFBD><C2BE><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>̡<EFBFBD>
g_imu_sample_time_us = mcu_time_us();
MPU_Get_Gyroscope(); //<2F>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MPU_Get_Accelscope(); //<2F><>ü<EFBFBD><C3BC>ٶȼ<D9B6>ֵ(ԭʼֵ)
}
}
//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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>ADC<EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><EFBFBD>µ<EFBFBD> FIFO<46><4F><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ax<EFBFBD><EFBFBD>ay<EFBFBD><EFBFBD>az<EFBFBD><EFBFBD>x<EFBFBD><EFBFBD>y<EFBFBD><EFBFBD>z<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>gx<EFBFBD><EFBFBD>gy<EFBFBD><EFBFBD>gz<EFBFBD><EFBFBD>x<EFBFBD><EFBFBD>y<EFBFBD><EFBFBD>z<EFBFBD><EFBFBD>Ǽ<EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><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>
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><C7B0><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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20><>ʱ<EFBFBD><CAB1>Դ
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>source<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><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){
I2C_WriteBits(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) {
I2C_WriteBits(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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20><><EFBFBD>ٶȼƵ<C8BC><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>range<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̱<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
//#define MPU6050_ACCEL_FS_2 0x00 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-2G
//#define MPU6050_ACCEL_FS_4 0x01 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-4G
//#define MPU6050_ACCEL_FS_8 0x02 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-8G
//#define MPU6050_ACCEL_FS_16 0x03 //===<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-16G
void MPU6050_setFullScaleAccelRange(uint8_t range) {
I2C_WriteBits(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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>˯<EFBFBD><CBAF>ģʽ
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>˯<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
I2C_WriteOneBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/**************************************************************************
Function: Read identity
Input : none
Output : 0x68
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡ MPU6050 WHO_AM_I <20><>ʶ
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>0x68
**************************************************************************/
uint8_t MPU6050_getDeviceID(void) {
return I2C_ReadOneByte(devAddr,MPU6050_RA_WHO_AM_I);
// 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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPU6050 <20>Ƿ<EFBFBD><C7B7>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD>ΪAUX I2C<32>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD>0<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
I2C_WriteOneBit(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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD>ΪAUX I2C<32>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD>0<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
I2C_WriteOneBit(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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD> MPU6050 <20>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
u8 MPU6050_initialize(void)
{
u8 res;
//IIC_Init(); //Initialize the IIC bus //<2F><>ʼ<EFBFBD><CABC>IIC<49><43><EFBFBD><EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80); //Reset MPUrobot_select_init.h //<2F><>λMPUrobot_select_init.h
delay_ms(200); //Delay 200 ms //<2F><>ʱ200ms
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00); //Wake mpurobot_select_init.h //<2F><><EFBFBD><EFBFBD>MPUrobot_select_init.h
//MPU6050_Set_Gyro_Fsr(1); //Gyroscope sensor //<2F><><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD><EFBFBD><EFBFBD>,<2C><>500dps=<3D><>500<30><30>/s <20><>32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_500);
//MPU6050_Set_Accel_Fsr(0); //Acceleration sensor //<2F><><EFBFBD>ٶȴ<D9B6><C8B4><EFBFBD><EFBFBD><EFBFBD>,<2C><>2g=<3D><>2*9.8m/s^2 <20><>32768 accel/32768*19.6=accel/1671.84
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
MPU6050_Set_Rate(50); //Set the sampling rate to 50Hz //<2F><><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD>50Hz
I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00); //Turn off all interrupts //<2F>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00); //The I2C main mode is off //I2C<32><43>ģʽ<C4A3>ر<EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00); //Close the FIFO //<2F>ر<EFBFBD>FIFO
//The INT pin is low, enabling bypass mode to read the magnetometer directly
//INT<4E><54><EFBFBD>ŵ͵<C5B5>ƽ<EFBFBD><C6BD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>bypassģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>Ӷ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_INT_PIN_CFG,0X80);
//Read the ID of MPU6050
//<2F><>ȡMPU6050<35><30>ID
res=I2C_ReadOneByte(devAddr,MPU6050_RA_WHO_AM_I);
if(res==MPU6050_DEFAULT_ADDRESS) //The device ID is correct, The correct device ID depends on the AD pin //<2F><><EFBFBD><EFBFBD>ID<49><44>ȷ, <20><><EFBFBD><EFBFBD>ID<49><44><EFBFBD><EFBFBD>ȷȡ<C8B7><C8A1><EFBFBD><EFBFBD>AD<41><44><EFBFBD><EFBFBD>
{
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X01); //Set CLKSEL,PLL X axis as reference //<2F><><EFBFBD><EFBFBD>CLKSEL,PLL X<><58>Ϊ<EFBFBD>ο<EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00); //Acceleration and gyroscope both work //<2F><><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
MPU6050_Set_Rate(50); //Set the sampling rate to 50Hz //<2F><><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD>Ϊ50Hz
}else return 1;
return 0;
}
/**************************************************************************
Function: Initialization of DMP in mpu6050
Input : none
Output : none
<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> ֵ<><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
<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> ֵ<><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>
// Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// Pitch = 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>
// Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //<2F><><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
// }
//}
/**************************************************************************
Function: Read mpu6050 built-in temperature sensor data
Input : none
Output : Centigrade temperature
<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> ֵ<><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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
Temp=(36.53f+Temp/340)*10; //<2F>¶ȷŴ<C8B7>ʮ<EFBFBD><CAAE><EFBFBD><EFBFBD><EFBFBD>
return (int)Temp;
}
/**************************************************************************
Function: Initialize TIM2 as the encoder interface mode
Input : LPF: Digital low-pass filtering frequency (Hz)
Output : 0: Settings successful, others: Settings failed
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPUrobot_select_init.h<><68><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>lpf:<3A><><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>Ƶ<EFBFBD><C6B5>(Hz)
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>0:<3A><><EFBFBD>óɹ<C3B3>, <20><><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
**************************************************************************/
unsigned char MPU6050_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return I2C_WriteOneByte(devAddr,MPU6050_RA_CONFIG,data); //Set the digital lowpass filter//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
}
/**************************************************************************
Function: Initialize TIM2 as the encoder interface mode
Input : rate:4~1000(Hz)
Output : 0: Settings successful, others: Settings failed
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPUrobot_select_init.h<>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD>(<28>ٶ<EFBFBD>Fs=1KHz)
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>rate:4~1000(Hz)
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>0:<3A><><EFBFBD>óɹ<C3B3>, <20><><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
**************************************************************************/
unsigned char MPU6050_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data); //Set the digital lowpass filter//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
return MPU6050_Set_LPF(rate/2); //Automatically sets LPF to half of the sampling rate //<2F>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>LPFΪ<46><CEAA><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>һ<EFBFBD><D2BB>
}
/**************************************************************************
Function: Initialize TIM2 as the encoder interface mode
Input : Gx, Gy, Gz: raw readings (plus or minus) of the x,y, and z axes of the gyroscope
Output : 0: success, others: error code
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ(ԭʼֵ)
**************************************************************************/
void MPU_Get_Gyroscope(void)
{
gyro[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L); //<2F><>ȡX<C8A1><58><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //<2F><>ȡY<C8A1><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //<2F><>ȡZ<C8A1><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //<2F><><EFBFBD><EFBFBD>ǰ10<31><30>
{
Led_Count=1; //LED high frequency flashing //LED<45><44>Ƶ<EFBFBD><C6B5>˸
Flag_Stop=1; //The software fails to flag location 1 //<2F><><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ<D6BE><CEBB>1
}
else //10 seconds after starting //<2F><><EFBFBD><EFBFBD>10<31><30><EFBFBD>
{
if(Deviation_Count==CONTROL_DELAY)
Flag_Stop=0; //The software fails to flag location 0 //<2F><><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ<D6BE><CEBB>0
Led_Count=300; //The LED returns to normal flicker frequency //LED<45>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˸Ƶ<CBB8><C6B5>
//Save the raw data to update zero by clicking the user button
//<2F><><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Original_gyro[0] =gyro[0];
Original_gyro[1] =gyro[1];
Original_gyro[2]= gyro[2];
//Removes zero drift data
//ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD>Ư<EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD>
gyro[0] =Original_gyro[0]-Deviation_gyro[0];
gyro[1] =Original_gyro[1]-Deviation_gyro[1];
gyro[2]= Original_gyro[2]-Deviation_gyro[2];
}
}
/**************************************************************************
Function: Initialize TIM2 as the encoder interface mode
Input : Gx, Gy, Gz: raw readings (plus or minus) of the x,y, and z axes of the gyroscope
Output : 0: success, others: error code
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><EFBFBD>ٶȼ<EFBFBD>ֵ(ԭʼֵ)
**************************************************************************/
void MPU_Get_Accelscope(void)
{
accel[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //<2F><>ȡX<C8A1><58><EFBFBD><EFBFBD>ٶȼ<D9B6>
accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //<2F><>ȡX<C8A1><58><EFBFBD><EFBFBD>ٶȼ<D9B6>
accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //<2F><>ȡZ<C8A1><5A><EFBFBD><EFBFBD>ٶȼ<D9B6>
if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //<2F><><EFBFBD><EFBFBD>ǰ10<31><30>
{
}
else //10 seconds after starting //<2F><><EFBFBD><EFBFBD>10<31><30><EFBFBD>
{
//Save the raw data to update zero by clicking the user button
//<2F><><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Original_accel[0] =accel[0];
Original_accel[1] =accel[1];
Original_accel[2]= accel[2];
//Removes zero drift data
//ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD>Ư<EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD>
accel[0] =Original_accel[0]-Deviation_accel[0];
accel[1] =Original_accel[1]-Deviation_accel[1];
accel[2]= Original_accel[2]-Deviation_accel[2]+16384;
}
}
//------------------End of File----------------------------
Reference in New Issue View Git Blame Copy Permalink