Created
October 28, 2021 06:42
-
-
Save tuo/cc403c024cdd1cd2c68965772f8376d6 to your computer and use it in GitHub Desktop.
MPU6050_angle_calibrate
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// source: http://www.brokking.net/imu.html MPU-6050 6dof IMU for auto-leveling multicopters | |
//Gyro angle calculations | |
//0.0000611 = 1 / (250Hz / 65.5) | |
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable | |
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable | |
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians | |
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel | |
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel | |
//Accelerometer angle calculations | |
acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //Calculate the total accelerometer vector | |
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians | |
angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296; //Calculate the pitch angle | |
angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296; //Calculate the roll angle | |
//Place the MPU-6050 spirit level and note the values in the following two lines for calibration | |
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch | |
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll | |
if (set_gyro_angles) | |
{ //If the IMU is already started | |
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle | |
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle | |
} | |
else | |
{ //At first start | |
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle | |
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle | |
set_gyro_angles = true; //Set the IMU started flag | |
} | |
//To dampen the pitch and roll angles a complementary filter is used | |
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value | |
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value | |
write_LCD(); //Write the roll and pitch values to the LCD display |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment