Skip to content

I wrote a library that measures pitch, roll and yaw angles with the help of a complementary filter.

Notifications You must be signed in to change notification settings

TalhaTelli427/MPU6050_Complementery_Filter_HAL_Lib

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

24 Commits
 
 
 
 

Repository files navigation

MPU6050_Complementery_Filter_HAL_Lib

Algorithm Block Diagram

Novohead Diyagram drawio

Circuit Image

Circuit Image

Test Video

ezgif-2-8768fdec2f

Detailed Description Of The Code

First, we perform the initialization settings required to use the MPU6050 sensor.

void TT_Init_MPU6050(void) {
// Power Conifg
data[0] = 0x00;
HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADRR, MPU6050_POWER_1, 1, data, 1, 10000);
// Internal Low Pass Filter Config
data[0] = 0x04;
HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADRR, MPU6050_CONFIG, 1, data, 1, 10000);
//Acceloremter Config +-8g
data[0] = 0x10;
HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADRR, MPU6050_ACCE_CONFIG, 1, data, 1,
		10000);
// Gyro Config ± 500 °/s
data[0] = 0x08;
HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADRR, MPU6050_GYRO_CONFIG, 1, data, 1,
		10000);
}

Then we read 2000 gyro values ​​and divide it by 2000 and after waiting 1000 ms, we turn on the green LED on the STM32. In this way, the user is informed that the system is ready for operation. The reason why we read 2000 gyro values ​​is to minimize the drift problem of the gyroscope sensor.

void TT_Get_Gyro_Cal_Values(void) {

for (int i = 0; i < 2000; i++) {

	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADRR, MPU6050_GYRO_MESURE, 1,
			Gyro_Cal_Buffer, 6, HAL_MAX_DELAY);

	Gyro_Cal_Raw_X = (Gyro_Cal_Buffer[0] << 8 | Gyro_Cal_Buffer[1]);
	Gyro_Cal_Raw_Y = (Gyro_Cal_Buffer[2] << 8 | Gyro_Cal_Buffer[3]);
	Gyro_Cal_Raw_Z = (Gyro_Cal_Buffer[4] << 8 | Gyro_Cal_Buffer[5]);

	Gyro_Cal_X += (float) Gyro_Cal_Raw_X;
	Gyro_Cal_Y += (float) Gyro_Cal_Raw_Y;
	Gyro_Cal_Z += (float) Gyro_Cal_Raw_Z;

	HAL_Delay(20);

	}

Gyro_Cal_X /= 2000;
Gyro_Cal_Y /= 2000;
Gyro_Cal_Z /= 2000;
HAL_Delay(1000);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, SET);

}

After the calibration data is obtained, the system makes a gyro reading and subtracts the calibration value from the raw gyro value read, thus minimizing the dirft problem. And the given in the datasheet is divided by the LSB sensitivity parameter.

void TT_Get_All_Values(void) {

		//Getting Gyro data
HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADRR, MPU6050_GYRO_MESURE, 1,
		Gyro_Buffer, 6, 100000);

Gyro_Raw_X = (Gyro_Buffer[0] << 8 | Gyro_Buffer[1]);
Gyro_Raw_Y = (Gyro_Buffer[2] << 8 | Gyro_Buffer[3]);
Gyro_Raw_Z = (Gyro_Buffer[4] << 8 | Gyro_Buffer[5]);

Gyro_Raw_X -= Gyro_Cal_X;
Gyro_Raw_Y -= Gyro_Cal_Y;
Gyro_Raw_Z -= Gyro_Cal_Z;

Gyro_X = Gyro_Raw_X / 65.5;
Gyro_Y = Gyro_Raw_Y / 65.5;
Gyro_Z = Gyro_Raw_Z / 65.5;

After taking the gyro value, the accelerometer values ​​are read and the read value is divided by the LSB sensitivity parameter given in the datasheet.

HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADRR, MPU6050_ACCE_MESURE, 1,
		Acce_Buffer, 6, 100000);

Acce_Raw_X = (Acce_Buffer[0] << 8 | Acce_Buffer[1]);
Acce_Raw_Y = (Acce_Buffer[2] << 8 | Acce_Buffer[3]);
Acce_Raw_Z = (Acce_Buffer[4] << 8 | Acce_Buffer[5]);

Acce_X = (float) Acce_Raw_X / 4096;
Acce_Y = (float) Acce_Raw_Y / 4096;
Acce_Z = (float) Acce_Raw_Z / 4096;

Using the acceleration values ​​obtained, the angle of the x and y axes to the ground is calculated. The obtained pitch and roll values ​​are passed through a low-pass filter. The pitch and roll angle values ​​calculate the tilt of the user's head to the ground. This allows the user to look up and down and down in our algorithm. The reason why we don't do it with an accelerometer is that the accelerometer sensor is very easily affected by noise and cannot calculate the rotations in the yaw axis. I used the low-pass filter to minimize the noise as much as possible.

    // Acce Angel Calculation
Mesured_Angle_Pitch_Acce = (atan(Acce_Y / sqrt(Acce_X * Acce_X + Acce_Z * Acce_Z)) * 1
			/ (3.142 / 180) + 1);
Mesured_Angle_Roll_Acce = (-atan(Acce_X / sqrt(Acce_Y * Acce_Y + Acce_Z * Acce_Z)) * 1
			/ (3.142 / 180) - 0.80);

New_Angel_Pitch_Acce=Old_Angle_Pitch_Acce*0.7+Mesured_Angle_Pitch_Acce*0.3;
New_Angel_Roll_Acce=Old_Angle_Roll_Acce*0.7+Mesured_Angle_Roll_Acce*0.3;
Old_Angle_Pitch_Acce=New_Angel_Pitch_Acce;
Old_Angle_Roll_Acce=New_Angel_Roll_Acce;

Even if we calibrate and filter the gyro and acceleration values, it is not enough. Accelerometer and gyros sensor have their own advantages and disadvantages. While the gyro sensor gives accurate results in the short term, it starts to make mistakes in the long term due to the drift problem. While the accelerometer makes accurate measurements in the long term, it is more affected by the noise in the short term and returns you an incorrect value. Complementary filter allows us to obtain the most accurate angle value by using both sensors.

// Complementery filter
Filterd_Pitch = (0.96f) * (Filterd_Pitch + Gyro_X * dt)
		+ (New_Angel_Pitch_Acce * 0.04f);
Filterd_Roll = (0.96f) * (Filterd_Roll + Gyro_Y * dt)
		+ (New_Angel_Roll_Acce *0.04f);

Since there is no magnetometer on the MPU6050 to filter and measure the yaw axis, it is passed through a high-pass filter and used that way.

Angle_Yaw_Gyro += Gyro_Z * dt * 0.98039215686274;

Since there is no magnetometer on the MPU6050 to filter and measure the yaw axis, it is passed through a high-pass filter and used that way.

//This process minimizes the noise applied by the axes to each other.
Filterd_Pitch += Filterd_Roll * sin(Gyro_Z * Omega);
Filterd_Roll -= Filterd_Pitch * sin(Gyro_Z * Omega);

Conclusion

As a result, thanks to an IMU sensor and microcontroller in the VR headset, we can monitor in real time the position of the head up and down, the tilted position to the right and left, and finally the position of turning the head left and right.

If you want to go to the Source. If you want to go to the Header. If you want to go to the Main.

About

I wrote a library that measures pitch, roll and yaw angles with the help of a complementary filter.

Topics

Resources

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published