Article directory
- 1, MPU6050 introduction
- 1. Relationship between mpu6050, gyroscope and accelerometer:
- 2. Overall summary
- 3. Pin description
- 4. Basic configuration and related registers
- 2, Code details
1. Relationship between mpu6050, gyroscope and accelerometer:
MPU6050 is a global first integrated 9-axis motion processing sensor launched by InvenSense company. Its biggest feature is that it eliminates the error of gyroscope and accelerometer, combines gyroscope and accelerometer together, and reduces space. The relationship between gyroscopes, accelerometers and MPU6050 has been described in a blog before: Blog links
2. Overall summary
The MPU6050 integrates three-axis MEMS gyroscope, three-axis MEMS accelerometer and an expandable digital motion processor DMP, and can also connect a third-party digital sensor (such as magnetometer), so that a 9-axis signal can be output through the IIC interface.
More convenient, with DMP, we can combine the motion processing database provided by InvenSense company to realize attitude calculation. Through the built-in DMP, the data of 9-axis fusion algorithm can be output through the IIC interface, which greatly reduces the load of motion processing operation on the operating system and reduces the development difficulty.
Characteristic:
① Output rotation matrix, quaternion and ohm of 6-axis or 9-axis (external magnetic sensor is required) in digital form
Fusion algorithm data of Euler angle forma (supported by DMP)
② With 131 LSBs / ° / sec sensitivity and full cell sensing range of ± 250, ± 500, ± 1000 and ± 2000 ° / sec
3-axis angular velocity sensor (gyroscope) of
③ Integrated programmable 3-axis acceleration sensor with range of ± 2g, ± 4g, ± 8g and ± 16g
④ Remove the sensitivity between the accelerator and the gyroscope axis, reduce the influence given by the setting and the drift of the sensor
⑤ DMP (digital motion processing) engine can reduce the complex fusion algorithm of MCU
Data, sensor synchronization, pose sensing, etc
⑥ The built-in operation time deviation and magnetic sensor calibration algorithm technology eliminate the need of customers for additional calibration
⑦ With a digital temperature sensor
⑧ With digital input sync pin to support video electronic phase stabilization technology and GPS
⑨ Programmable interrupt, which supports gesture recognition, panning, picture zooming, scrolling and rapid descent
Interrupt, high-G interrupt, zero motion sensing, touch sensing, shake sensing functions
⑩ VDD supply voltage is 2.5V ± 5%, 3.0V ± 5%, 3.3V ± 5%; VLOGIC can be as low as 1.8V ± 5%
⑪ Working current of gyroscope: 5mA, standby current of gyroscope: 5uA, working current of accelerator: 500uA, accelerator saving
Electric mode current: 40uA@10Hz
⑫ With 1024 byte FIFO, it helps to reduce system power consumption
⑬ IIC communication interface up to 400Khz
⑭ Ultra small package size: 4x4x0.9mm (QFN)
The detection axis is as shown in the figure:
3. Pin description
As shown in the figure, the MPU6050 has eight pins in total. In fact, when outputting six axis data, only five pins are used: VCC, GND, SCL, SDA and AD0. Here are the pins:
- VCC: power supply, 3.3V
- GND: grounding.
- SCL: IIC clock interface to MCU
- SAD: IIC data interface to MCU
- XCL: IIC clock interface to external devices
- XAD: IIC data interface for connecting external devices
- AD0: address control pin (lowest control address)
- INT: interrupt trigger interface (not used)
XCL and XDA can only be used when connecting external equipment (such as magnetometer). AD0 is used to control the address of MPU6050. If AD0 is low, the address is 0X68; if AD0 is high, the address is 0X69.
4. Basic configuration and related registers
The communication between MCU and MPU6050 is based on IIC communication mechanism. On the basis of IIC, the operation of MPU6050 register can be realized, and the operation of MPU6050 is to read and write registers. Therefore, it is necessary to understand the relevant registers and the operation of registers. The register related information of MPU6050 can be found in the data manual. Here are some important registers:
Power management register 1
Address: 0X68
Main bit function:
- Device "reset: control reset, 1 means reset, and it will reset automatically after reset;
- SLEEP: control MPU6050 working mode, 1 represents SLEEP mode, 0 represents normal working mode, reset to 1, and reset to zero manually;
- Temp? Dis: enable temperature sensor bit, 0 represents enable;
- CLKSEL[2:0]: select the clock source of the system. Generally, PLL ﹣ u X-axis gyro is used as the reference. The specific clock source selection and corresponding bit values are shown in the figure:
Gyroscope configuration register
Address: 0X1B
Main bit function:
- FS_SEL[1:0]: set the full range range of gyroscope: 0 for ± 250 ° / s, 1 for ± 500 ° / s, 2 for ± 1000 ° / s and 3 for ± 2000 ° / S;
The resolution of gyroscope is 16 bits, so the sensitivity under the maximum range is 65536/4000=16.4LSB / (° S).
Accelerometer configuration register
Address: 0X1C
Main bit function:
- AFS_SEL[1:0]: set the full scale range of accelerometer: 0 for ± 2g, 1 for ± 4g, 2 for ± 8g, 3 for ± 16g;
FIFO enable register
Address: 0X23
It is used to control the FIFO function. The corresponding bit corresponds to the corresponding sensor FIFO function. If it is 0, it means forbidden, and if it is 1, it means enabled. Note: the three axis FIFO function of the acceleration sensor is controlled by one bit accel ﹐ FIFO ﹐ en. In the case of simply reading sensor data, FIFO is not used.
Frequency division register of gyro sampling rate
Address: 0X19
The register is changed to set the sampling frequency of MPU6050 gyroscope, and the output frequency of the gyroscope is related to it. The relationship between the two is: sampling frequency = gyroscope output frequency / (1 + smplrt? DIV)
The output frequency of gyroscope is related to the digital low pass filter (DLPF), which is usually set to half of the sampling rate.
Temperature sensor register
Address: high octet 0X41, low octet 0X42
Directly read the value in the register to get the temperature data. The temperature conversion formula is:
Temperature = 36.53 + regval/340
1. framework
I use STM32 to drive MPU6050. MPU6050 outputs the original six axis data. After DMP processing (with database), I get quaternion, and then calculate the Euler angle from quaternion: yaw, roll, pitch. It is printed on the computer screen by serial port.
- First of all, I need to do the underlying IIC driver to establish communication with MPU6050, which I implemented in MPU · IIC. C;
- Then, with the underlying driver, it is necessary to write some functions to communicate with MPU6050 (by reading and writing registers), write commands, configure MPU6050, read raw data, etc. I write these operations in mpu6050.c, of course, the mpu6050.h header file also contains the address of each register and related instructions of MPU.
- Through the implementation of mpu6050.c, the original six axis data can be read out. The next step is to convert the original data to quaternion through DMP. In this step, the DMP algorithm is limited in my level, and only the routines provided by InvenSense company can be transplanted. As for the transplanted DMP algorithm, because the essence of DMP algorithm is also the operation of MPU6050, we only need to provide the transplanted algorithm with the function interface of reading and writing MPU6050 register, and finally read out the quaternion directly through the transplanted function!
- Then, the quaternion is transformed into Euler angle, which is relatively simple and can be realized by one function.
- Finally, print the serial port to the screen.
Here are the function files
2.mpu_iic.c/mpu_iic.h
mpu_iic.h
It mainly defines the operation of pin level and function declaration.
#ifndef __MPU_IIC_H #define __MPU_IIC_H #include "stm32f10x.h" #include "delay.h" /* Macro defined pin level operation function */ #define MPU_SDA_IN() #define MPU_SDA_OUT() #define MPU_IIC_SDA_1() GPIO_SetBits( GPIOB, GPIO_Pin_11 ) #define MPU_IIC_SDA_0() GPIO_ResetBits( GPIOB, GPIO_Pin_11 ) #define MPU_IIC_SCL_1() GPIO_SetBits( GPIOB, GPIO_Pin_10 ) #define MPU_IIC_SCL_0() GPIO_ResetBits( GPIOB, GPIO_Pin_10 ) #define MPU_IIC_AD0_1() GPIO_SetBits( GPIOA, GPIO_Pin_15 ) #define MPU_IIC_AD0_0() GPIO_ResetBits( GPIOA, GPIO_Pin_15 ) #define MPU_IIC_SDA_READ() GPIO_ReadInputDataBit( GPIOB, GPIO_Pin_11 ) #define MPU_IIC_Delay() delay_us(2) /* Function declaration */ void MPU_IIC_Init( void ); void MPU_IIC_Start( void ); void MPU_IIC_Stop( void ); uint8_t MPU_IIC_Wait_Ack( void ); void MPU_IIC_Ack( void ); void MPU_IIC_NAck( void ); void MPU_IIC_Send_Byte( uint8_t data ); uint8_t MPU_IIC_Read_Byte( uint8_t ack ); #endif
mpu_iic.c
There is nothing to say about simulating IIC code through software.
#include "mpu_iic.h" #include "usart.h" /* IIC Interface pin configuration SDA:PB11 SCL:PB10 AD0:PB2 */ void MPU_IIC_Init( void ) { GPIO_InitTypeDef GPIO_InitStruct; RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE ); GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; GPIO_Init( GPIOB, &GPIO_InitStruct ); MPU_IIC_SDA_1(); MPU_IIC_SCL_1(); } void MPU_IIC_Start( void ) { MPU_SDA_OUT(); MPU_IIC_SDA_1(); MPU_IIC_SCL_1(); delay_us(2); MPU_IIC_SDA_0(); delay_us(2); MPU_IIC_SCL_0(); } void MPU_IIC_Stop( void ) { MPU_SDA_OUT(); MPU_IIC_SDA_0(); MPU_IIC_SCL_1(); delay_us(2); MPU_IIC_SDA_1(); MPU_IIC_SCL_1(); delay_us(2); } /* The slave will pull down the SDA as the response when the SCL is high Return value: 1: no response 0: Responded */ uint8_t MPU_IIC_Wait_Ack( void ) { uint8_t count; MPU_SDA_IN(); MPU_IIC_SCL_1(); delay_us(2); MPU_IIC_SDA_1(); delay_us(2); while( MPU_IIC_SDA_READ()==1 ) { count++; if( count>250 ) { MPU_IIC_Stop(); return 1; } } MPU_IIC_SCL_0(); return 0; } void MPU_IIC_Ack( void ) { MPU_IIC_SCL_0(); MPU_SDA_OUT(); MPU_IIC_SDA_0(); delay_us(2); MPU_IIC_SCL_1(); delay_us(2); MPU_IIC_SCL_0(); } void MPU_IIC_NAck( void ) { MPU_IIC_SCL_0(); MPU_SDA_OUT(); MPU_IIC_SDA_1(); delay_us(2); MPU_IIC_SCL_1(); delay_us(2); MPU_IIC_SCL_0(); } /* Send a byte of data, high first */ void MPU_IIC_Send_Byte( uint8_t data ) { uint8_t t; MPU_SDA_OUT(); MPU_IIC_SCL_0(); for( t=0;t<8;t++ ) { if( ((data&0x80)>>7)==1 ) MPU_IIC_SDA_1(); else MPU_IIC_SDA_0(); data<<=1; MPU_IIC_SCL_1(); delay_us(2); MPU_IIC_SCL_0(); delay_us(2); } } /* When a byte is read, ack=1, the host sends a response after reading */ uint8_t MPU_IIC_Read_Byte( uint8_t ack ) { uint8_t t,data=0; MPU_SDA_IN(); for( t=0;t<8;t++ ) { MPU_IIC_SCL_0(); delay_us(2);//Waiting for SDA changes MPU_IIC_SCL_1(); data<<=1;//Must be before the read, because the next bit will not shift after reading if( MPU_IIC_SDA_READ()==1 ) data++; delay_us(2);//Waiting for SDA changes } if( !ack ) MPU_IIC_NAck();//Send nACK else MPU_IIC_Ack(); //Send ACK return data; }
3.mpu6050.c/mpu6050.h
mpu6050.h
It mainly defines the address of MPU related registers and makes function declaration.
#ifndef __MPU6050_H #define __MPU6050_H #include "stm32f10x.h" #include "mpu_iic.h" /* AD0 Ground, if the IIC address of MPU6050 is 0x68 and 3.3V is 0x69*/ #define MPU_ADDR 0X68 /************** MPU6050 Address of correlation register*********************/ #define MPU_ACCEL_OFFS_REG 0X06 //Accel? Off register, readable version number, not mentioned in register manual #define MPU_PROD_ID_REG 0X0C //prod id register, not mentioned in register manual #define MPU_SELF_TESTX_REG 0X0D //Self check register X #define MPU_SELF_TESTY_REG 0X0E //Self check register Y #define MPU_SELF_TESTZ_REG 0X0F //Self check register Z #define MPU_SELF_TESTA_REG 0X10 //Self check register A #define MPU_SAMPLE_RATE_REG 0X19 //Sampling frequency divider #define MPU_CFG_REG 0X1A //Configuration register #define MPU_GYRO_CFG_REG 0X1B //Gyroscope configuration register #define MPU_ACCEL_CFG_REG 0X1C //Accelerometer configuration register #define MPU_MOTION_DET_REG 0X1F //Motion detection threshold setting register #define MPU_FIFO_EN_REG 0X23 //FIFO enable register #define MPU_I2CMST_CTRL_REG 0X24 //IIC host control register #define MPU_I2CSLV0_ADDR_REG 0X25 //IIC slave 0 device address register #define MPU_I2CSLV0_REG 0X26 //IIC slave 0 data address register #define MPU_I2CSLV0_CTRL_REG 0X27 //IIC slave 0 control register #define MPU_I2CSLV1_ADDR_REG 0X28 //IIC slave 1 device address register #define MPU_I2CSLV1_REG 0X29 //IIC slave 1 data address register #define MPU_I2CSLV1_CTRL_REG 0X2A //IIC slave 1 control register #define MPU_I2CSLV2_ADDR_REG 0X2B //IIC slave 2 device address register #define MPU_I2CSLV2_REG 0X2C //IIC slave 2 data address register #define MPU_I2CSLV2_CTRL_REG 0X2D //IIC slave 2 control register #define MPU_I2CSLV3_ADDR_REG 0X2E //IIC slave 3 device address register #define MPU_I2CSLV3_REG 0X2F //IIC slave 3 data address register #define MPU_I2CSLV3_CTRL_REG 0X30 //IIC slave 3 control register #define MPU_I2CSLV4_ADDR_REG 0X31 //IIC slave 4 device address register #define MPU_I2CSLV4_REG 0X32 //IIC slave 4 data address register #define MPU_I2CSLV4_DO_REG 0X33 //IIC slave 4 write data register #define MPU_I2CSLV4_CTRL_REG 0X34 //IIC slave 4 control register #define MPU_I2CSLV4_DI_REG 0X35 //IIC slave 4 read data register #define MPU_I2CMST_STA_REG 0X36 //IIC host status register #define MPU_INTBP_CFG_REG 0X37 //Interrupt / bypass setting register #define MPU_INT_EN_REG 0X38 //Interrupt enable register #define MPU_INT_STA_REG 0X3A //Interrupt status register #define MPU_ACCEL_XOUTH_REG 0X3B //Acceleration value, X-axis high 8-bit register #define MPU_ACCEL_XOUTL_REG 0X3C //Acceleration value, X-axis low 8-bit register #define MPU_ACCEL_YOUTH_REG 0X3D //Acceleration value, Y-axis high 8-bit register #define MPU_ACCEL_YOUTL_REG 0X3E //Acceleration value, Y-axis low 8-bit register #define MPU_ACCEL_ZOUTH_REG 0X3F //Acceleration value, Z-axis high 8-bit register #define MPU_ACCEL_ZOUTL_REG 0X40 //Acceleration value, Z-axis low 8-bit register #define MPU_TEMP_OUTH_REG 0X41 //Temperature value high octet register #define MPU_TEMP_OUTL_REG 0X42 //Temperature value low 8-bit register #define MPU_GYRO_XOUTH_REG 0X43 //Gyroscope value, X-axis high 8-bit register #define MPU_GYRO_XOUTL_REG 0X44 //Gyroscope value, X-axis low 8-bit register #define MPU_GYRO_YOUTH_REG 0X45 //Gyroscope value, Y-axis high 8-bit register #define MPU_GYRO_YOUTL_REG 0X46 //Gyroscope value, Y-axis low 8-bit register #define MPU_GYRO_ZOUTH_REG 0X47 //Gyroscope value, Z-axis high 8-bit register #define MPU_GYRO_ZOUTL_REG 0X48 //Gyroscope value, Z-axis low 8-bit register #define MPU_I2CSLV0_DO_REG 0X63 //IIC slave 0 data register #define MPU_I2CSLV1_DO_REG 0X64 //IIC slave 1 data register #define MPU_I2CSLV2_DO_REG 0X65 //IIC slave 2 data register #define MPU_I2CSLV3_DO_REG 0X66 //IIC slave 3 data register #define MPU_I2CMST_DELAY_REG 0X67 //IIC host delay management register #define MPU_SIGPATH_RST_REG 0X68 //Signal channel reset register #define MPU_MDETECT_CTRL_REG 0X69 //Motion detection control register #define MPU_USER_CTRL_REG 0X6A //User control register #define MPU_PWR_MGMT1_REG 0X6B //Power management register 1 #define MPU_PWR_MGMT2_REG 0X6C //Power management register 2 #define MPU_FIFO_CNTH_REG 0X72 //FIFO count register high octet #define MPU_FIFO_CNTL_REG 0X73 //FIFO count register low octet #define MPU_FIFO_RW_REG 0X74 //FIFO read write register #define MPU_DEVICE_ID_REG 0X75 //Device ID register /* Function declaration */ uint8_t MPU_Read_Byte( uint8_t reg ); uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data ); uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf ); uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf ); uint8_t MPU_Init( void ); uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr ); uint8_t MPU_Set_Accel_Fsr( uint8_t fsr ); uint8_t MPU_Set_LPF( uint16_t lpf ); uint8_t MPU_Set_Rate( uint16_t rate ); short MPU_Get_Temperature( void ); uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz ); uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az ); #endif
mpu6050.c
The most important part of the code, including a series of basic configuration of MPU6050 and the operation of reading the original data, is annotated in detail.
The AD0 pin uses PA15, so if you want to GPIO ﹣ remap ﹣ swj ﹣ jtagdisable, you can change the pin.
#include "mpu6050.h" #include "usart.h" /** * @brief : Initialize MPU * @param : None * @retval : 0: Initialization complete */ uint8_t MPU_Init( void ) { uint8_t k=1; uint8_t res; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//Enable AFIO clock RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//Enable peripheral IO PORTA clock first GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; // port configuration GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //Push-pull output GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO port speed is 50MHz GPIO_Init(GPIOA, &GPIO_InitStructure); //Initialize GPIOA according to set parameters GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); MPU_IIC_AD0_0(); MPU_IIC_Init(); /* Set MPU6050 address to 0X68 and initialize IIC bus */ /* MPU_PWR_MGMT1_REG: Power management register */ MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X80 );//Reset MPU6050 // printf("read register value:% 02x \ n", MPU read byte (MPU PWR mgmt1 reg)); delay_ms(100); MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X00 );//Wake up MPU6050 /* Set gyroscope range: ± 2000dps Set accelerometer range: ± 2g Set sampling frequency: 50Hz (low pass filter frequency 100Hz) */ MPU_Set_Gyro_Fsr( 3 ); MPU_Set_Accel_Fsr( 0 ); MPU_Set_Rate( 50 ); MPU_Write_Byte( MPU_INT_EN_REG, 0X00 ); //Turn off all interrupts MPU_Write_Byte( MPU_USER_CTRL_REG, 0X00 );//Turn off IIC main mode MPU_Write_Byte( MPU_FIFO_EN_REG, 0X00 ); //Close FIFO MPU_Write_Byte( MPU_INTBP_CFG_REG, 0X80 );//Set INT pin low level valid res = MPU_Read_Byte( MPU_DEVICE_ID_REG ); //Read MPU6050ID printf("ID:%X\n",res); /* Confirm ID */ if( res==MPU_ADDR ) { MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X01 );//PLL X axis as clock reference MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0X00 );//Enable gyroscopes and accelerometers MPU_Set_Rate( 50 ); return 0; } else return 1; } /** * @brief : Read the value of a register (8 bits) * @param : reg: Register address * @retval : Read register data */ uint8_t MPU_Read_Byte( uint8_t reg ) { uint8_t data; MPU_IIC_Start(); /* Send MPU6050 device address and write command (the lowest bit is 0) */ MPU_IIC_Send_Byte( (MPU_ADDR<<1)|0 ); MPU_IIC_Wait_Ack(); /* Write register address */ MPU_IIC_Send_Byte( reg ); MPU_IIC_Wait_Ack(); MPU_IIC_Start(); /* Send read command to MPU6050 */ MPU_IIC_Send_Byte( (MPU_ADDR<<1)|1 ); MPU_IIC_Wait_Ack(); /* Read the value of the register */ data = MPU_IIC_Read_Byte( 0 ); MPU_IIC_Stop(); return data; } /** * @brief : Write data in the specified register (8 bits) * @param : reg: Register address data: Data to write to register * @retval : Normal return 0 */ uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data ) { MPU_IIC_Start(); /* Send MPU6050 device address and write command (the lowest bit is 0) */ MPU_IIC_Send_Byte( (MPU_ADDR<<1)|0 ); if( MPU_IIC_Wait_Ack() ) { MPU_IIC_Stop(); return 1; } /* Write register address */ MPU_IIC_Send_Byte( reg ); MPU_IIC_Wait_Ack(); /* Send data to write */ MPU_IIC_Send_Byte( data ); if( MPU_IIC_Wait_Ack() ) { MPU_IIC_Stop(); return 1; } MPU_IIC_Stop(); return 0; } /** * @brief : Read the data in the register continuously * @param : reg: Register address len: Length of data to read in bytes *buf: Store read data * @retval : Normal return 0 */ uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf ) { MPU_IIC_Start(); /* Send MPU6050 device address and write command (the lowest bit is 0) */ MPU_IIC_Send_Byte( (addr<<1)|0 ); if( MPU_IIC_Wait_Ack() ) { MPU_IIC_Stop(); return 1; } /* Write register address */ MPU_IIC_Send_Byte( reg ); MPU_IIC_Wait_Ack(); MPU_IIC_Start(); /* Send read command to MPU6050 */ MPU_IIC_Send_Byte( (MPU_ADDR<<1)|1 ); MPU_IIC_Wait_Ack(); while( len ) { /* If read only one bit, do not send reply */ if( len==1 ) *buf = MPU_IIC_Read_Byte( 0 ); else *buf = MPU_IIC_Read_Byte( 1 ); len--; buf++; } MPU_IIC_Stop(); return 0; } /** * @brief : Write data continuously in register * @param : reg: Register address len: Length of data to write in bytes *buf: Data to write to register * @retval : Normal return 0 */ uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf ) { uint8_t i; MPU_IIC_Start(); /* Send MPU6050 device address and write command (the lowest bit is 0) */ MPU_IIC_Send_Byte( (addr<<1)|0 ); if( MPU_IIC_Wait_Ack() ) { MPU_IIC_Stop(); return 1; } /* Write register address */ MPU_IIC_Send_Byte( reg ); MPU_IIC_Wait_Ack(); i=0; while( len ) { MPU_IIC_Send_Byte( buf[i] ); i++; if( MPU_IIC_Wait_Ack() ) { MPU_IIC_Stop(); return 1; } len--; } MPU_IIC_Stop(); return 0; } /** * @brief : Set gyroscope range * @param : fsr:0: ±250dps 1: ±500dps 2: ±1000dps 3: ±2000dps * @retval : 0: Set up successfully 1: Setup failed */ uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr ) { return MPU_Write_Byte( MPU_GYRO_CFG_REG, fsr<<3 ); } /** * @brief : Set accelerometer range * @param : fsr:0: ±2g 1: ±4g 2: ±8g 3: ±16g * @retval : 0: Set up successfully 1: Setup failed */ uint8_t MPU_Set_Accel_Fsr( uint8_t fsr ) { return MPU_Write_Byte( MPU_ACCEL_CFG_REG, fsr<<3 ); } /** * @brief : Set low pass filter frequency * @param : Hz: frequency * @retval : 0: Set up successfully 1: Setup failed */ uint8_t MPU_Set_LPF( uint16_t lpf ) { uint8_t 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 MPU_Write_Byte(MPU_CFG_REG,data); } /** * @brief : Set sampling frequency * @param : Hz: 4~1000Hz * @retval : 0: Set up successfully 1: Setup failed */ uint8_t MPU_Set_Rate( uint16_t rate ) { uint8_t data; if(rate>1000) rate=1000; if(rate<4) rate=4; data=1000/rate-1; /* sampling frequency */ data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); return MPU_Set_LPF(rate/2); //Automatically set LPF to half the sampling rate } /** * @brief : Obtain temperature value * @param : None * @retval : 100 times higher temperature (actually to keep two decimal places) */ short MPU_Get_Temperature( void ) { uint8_t buf[2]; uint16_t raw;//Store original temperature value float temp; /* MPU_TEMP_OUTH_REG: High octet register 0X41 of temperature value MPU_TEMP_OUTL_REG: Temperature value low octet register 0X42 Read 16 bit temperature value continuously*/ MPU_Read_Continue( MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf ); /* Read the original temperature value */ raw = ( ( uint16_t )buf[0]<<8 )|buf[1]; /* Conversion temperature value */ temp = 36.53+( (double)raw )/340; /* Two decimal places are reserved, and the decimal part will be removed when the decimal is converted to an integer */ return temp*100; } /** * @brief : Get gyroscope value * @param : gx,gy,gz: Is the original reading of the x, y, z axis of the gyroscope (16 bits) * @retval : 0: Get success 1: Acquisition failure */ uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz ) { uint8_t buf[6],res; /* MPU_GYRO_XOUTH_REG: x Shaft height octet register The register addresses of the three axes are continuous, first high, then low */ if( (res=MPU_Read_Continue( MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf ))==0 ) { *gx = ((uint16_t)buf[0]<<8)|buf[1]; *gy = ((uint16_t)buf[2]<<8)|buf[3]; *gz = ((uint16_t)buf[4]<<8)|buf[5]; } return res; } /** * @brief : Get accelerometer value * @param : ax,ay,az: Is the original reading of the x, y, z axis of the accelerometer (16 bits) * @retval : 0: Get success 1: Acquisition failure */ uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az ) { uint8_t buf[6],res; /* MPU_ACCEL_XOUTH_REG: x Shaft height octet register The register addresses of the three axes are continuous, first high, then low */ if( (res=MPU_Read_Continue( MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf ))==0 ) { *ax = ((uint16_t)buf[0]<<8)|buf[1]; *ay = ((uint16_t)buf[2]<<8)|buf[3]; *az = ((uint16_t)buf[4]<<8)|buf[5]; } return res; }
4.DMP related codes
If you want to use DMP to find the Euler angle code, you can include the following files. The interface functions are listed below. When you use them, you can directly use the interface functions.
Interface macro definition provided to DMP algorithm
Only need to provide: read and write operation function and delay function of MPU6050
#define i2c_write MPU_Write_Continue #define i2c_read MPU_Read_Continue #define delay_ms delay_ms
DMP initialization
//mpu6050,dmp initialization //Return value: 0, normal // Other, failed uint8_t mpu_dmp_init(void) { uint8_t res=0; MPU_IIC_Init(); //Initialize IIC bus if(mpu_init()==0) //Initialize MPU6050 { res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//Set the required sensor if(res)return 1; res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//Set FIFO if(res)return 2; res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //Set sample rate if(res)return 3; res=dmp_load_motion_driver_firmware(); //Load dmp firmware if(res)return 4; res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//Set gyroscope direction if(res)return 5; res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //Set dmp function DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| DMP_FEATURE_GYRO_CAL); if(res)return 6; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //Set DMP output rate (up to 200Hz) if(res)return 7; res=run_self_test(); //Self checking if(res)return 8; res=mpu_set_dmp_state(1); //Enabling DMP if(res)return 9; }else return 10; return 0; }
Obtain Euler angle
//Get the data processed by dmp (note that this function needs more stacks and more local variables) //Pitch: pitch angle accuracy: 0.1 ° range: - 90.0 ° < --- > + 90.0 ° //Roll: roll angle accuracy: 0.1 ° range: - 180.0 ° < --- > + 180.0 ° //yaw: heading angle accuracy: 0.1 ° range: - 180.0 ° < --- > + 180.0 ° //Return value: 0, normal // Other, failed uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw) { float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units. * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent. **/ /*if (sensors & INV_XYZ_GYRO ) send_packet(PACKET_TYPE_GYRO, gyro); if (sensors & INV_XYZ_ACCEL) send_packet(PACKET_TYPE_ACCEL, accel); */ /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30. * The orientation is set by the scalar passed to dmp_set_orientation during initialization. **/ if(sensors&INV_WXYZ_QUAT) { q0 = quat[0] / q30; //q30 format to floating point q1 = quat[1] / q30; q2 = quat[2] / q30; q3 = quat[3] / q30; //Calculate the pitch angle / roll angle / heading angle *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw }else return 2; return 0; }
5.mian() function
After the MPU6050 is initialized in the main function, DMP is also initialized. Then you can directly use MPU DMP get data() to get the Euler angle and the temperature value.
#include "stm32f10x.h" #include "usart.h" #include "delay.h" #include "mpu6050.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" int main(void) { uint8_t x=0; float pitch,roll,yaw; //Euler angles short aacx,aacy,aacz; //Acceleration sensor raw data short gyrox,gyroy,gyroz; //Original data of gyroscope short temp; //temperature NVIC_PriorityGroupConfig( 2 ); delay_init(); USART1_Init(115200); printf("Program start\n"); if( MPU_Init()!=0 ) { printf("MPU6050 Initialization error!\n"); return 0; } if( mpu_dmp_init() ) { printf("DMP Initialization error!\n"); return 0; } while(1) { if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0) { temp=MPU_Get_Temperature(); //Get the temperature value MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //Get acceleration sensor data MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //Get gyroscope data } delay_ms(100); printf("pitch:%02f roll:%02f yaw:%02f\n",pitch,roll,yaw); } }
This is the MPU6050 that I understand. When doing the balance car later, I need to read the Euler angle. First, I want to summarize. If there is any problem, I can communicate with you and make progress together!!!
q: 2723808286
Crayon Xiaoxin has no blog 63 original articles published, 188 praised, 20000 visitors+ Private letter follow