I recently bought a MPU 6050 that I plan to use with a drone. However I face some difficulties now because the MPU is not giving me steady results.

The results (pitch, roll, yaw) i get are not consistent. If I for example keep the chip stable on a table the value keeps increasing/decreasing even though it is not being touched/moved.

I also seem to get different results if i restart it even though i use it at the same angles.

Is something off with my code or is there anything else I need to do in order to fix this issue?

Appreciate any help I can get! :)

Here is my current code:

#include <Servo.h> 
#include <Wire.h>

int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;

void setup() {


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

gyro_x_cal += gyro_x;     
gyro_y_cal += gyro_y; 
gyro_z_cal += gyro_z;

gyro_x_cal /= 2000;
gyro_y_cal /= 2000;  
gyro_z_cal /= 2000;   
loop_timer = micros(); 


void loop(){


gyro_x -= gyro_x_cal;           
gyro_y -= gyro_y_cal;                                    
gyro_z -= gyro_z_cal;

angle_pitch += gyro_x * 0.0000611;  
angle_roll += gyro_y * 0.0000611;  

angle_pitch += angle_roll * sin(gyro_z * 0.000001066);   
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); 

acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; 
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;     

angle_pitch_acc -= 0.0;                      
angle_roll_acc -= 0.0;                  

angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
angle_pitch = angle_pitch_acc;                                   
angle_roll = angle_roll_acc;                                 
set_gyro_angles = true; 


while(micros() - loop_timer < 4000); 
loop_timer = micros(); 

void read_mpu_6050_data(){    

while(Wire.available() < 14);          
acc_x = Wire.read()<<8|Wire.read();           
acc_y = Wire.read()<<8|Wire.read();             
acc_z = Wire.read()<<8|Wire.read();              
temperature = Wire.read()<<8|Wire.read();                 
gyro_x = Wire.read()<<8|Wire.read();               
gyro_y = Wire.read()<<8|Wire.read();             
gyro_z = Wire.read()<<8|Wire.read();                     


void setup_mpu_6050_registers(){


  • Did you put together pieces of code you found on the internet ? Can you tell where you found them ? It is a confusing mix of different programming styles and the formulas, the usage of the Wire library, the mixing of int, long and float are also confusing. The MPU-9250 has a magnetometer for the yaw. There are a number of good libraries for sensor fusion with filters for the 6DOF or 9DOF sensors. The MPU-6050 and MPU-9250 can also use the internal dmp firmware. You could start with the Sparkfun.com website and search for MPU-9250.
    – Jot
    Mar 25, 2017 at 21:56
  • I do not use mpu-9250, i use the mpu-6050. And yea when it comes to the code i have found it everywhere and put together something on my own also which makes it a bit chaotic!
    – Martman
    Mar 25, 2017 at 22:04
  • The MPU-6050 is outdated and the modules on Ebay could have a wrong capacitor making it even more noisier. Can you give a reference for those formulas ? Without reference I have a lot of doubts. Looking at the code, I even have doubts about the 4ms delay. Your loop is probably more slowed down by the full output buffer of Serial than by micros(). I strongly suggest to use a library and you might consider to buy a MPU-9250. Are you perhaps using a 5V Arduino without I2C level shifter ?
    – Jot
    Mar 25, 2017 at 22:40
  • I bought it from ebay so perhaps the quality is not great :( My arduino has a I2C i believe. Do you think this example could work well? github.com/jarzebski/Arduino-MPU6050/blob/master/…
    – Martman
    Mar 25, 2017 at 23:03
  • I see some problems: the use of the Wire library is wrong (a endTransmission after requestFrom, a while loop after requestFrom); no sensor fusion; shifting a 8-bit variable 8 times to the left. This one tests new formulas. I think the results are a little noisy. Remove the display calls: github.com/kriswiner/MPU-6050/blob/master/MPU6050IMU.ino This one works: forum.arduino.cc/index.php?topic=58048.0 This one is most often used: github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 but there have been some complaints that it does not use a real Kalman filter.
    – Jot
    Mar 27, 2017 at 3:04


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.