STM32 - driving six shaft MPU6050 output Euler angle

Article directory

1, MPU6050 introduction

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

2, Code details

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()  {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}  


#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

63 original articles published, 188 praised, 20000 visitors+
Private letter follow

Tags: Database angular

Posted on Sat, 15 Feb 2020 05:42:45 -0500 by besly98