Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 5 of 5

Thread: MPU6050 Sketch dont work well

  1. #1
    Junior Member
    Join Date
    Aug 2020
    Posts
    12

    MPU6050 Sketch dont work well

    Hi,

    i have a problem with the MPU 6050 Sketch that i tried. I tried it on a Arduino Nano and everything worked well. But on the Teensy the output is nan. And when i change the angle_pitch to a long, the nan disapear and the outputs are numbers. But also the numbers were not correct. But on the Arduino it works also with a float and on the Arduino it got every angle right. Does anyone now, what the problem could be?

    Thanks

    Code:
    #include <Wire.h>
    //Declaring some global variables
    int gyro_x, gyro_y, gyro_z;
    long gyro_x_cal, gyro_y_cal, gyro_z_cal;
    boolean set_gyro_angles;
    
    long acc_x, acc_y, acc_z, acc_total_vector;
    float angle_roll_acc, angle_pitch_acc;
    float a = 0;
    float angle_pitch, angle_roll;
    int angle_pitch_buffer, angle_roll_buffer;
    float angle_pitch_output, angle_roll_output;
    
    long loop_timer;
    int temp;
    
    void setup() {
      delay(10);
      Wire.begin();                                                        //Start I2C as master
      setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050 
      for (int cal_int = 0; cal_int < 1000 ; cal_int ++){                  //Read the raw acc and gyro data from the MPU-6050 for 1000 times
        read_mpu_6050_data();                                             
        gyro_x_cal += gyro_x;                                              //Add the gyro x offset to the gyro_x_cal variable
        gyro_y_cal += gyro_y;                                              //Add the gyro y offset to the gyro_y_cal variable
        gyro_z_cal += gyro_z;                                              //Add the gyro z offset to the gyro_z_cal variable
        delay(3);                                                          //Delay 3us to have 250Hz for-loop
      }
    
      // divide by 1000 to get avarage offset
      gyro_x_cal /= 1000;                                                 
      gyro_y_cal /= 1000;                                                 
      gyro_z_cal /= 1000;                                                 
      Serial.begin(115200);
      loop_timer = micros();                                               //Reset the loop timer
    }
    
    void loop(){
    
      read_mpu_6050_data();   
     //Subtract the offset values from the raw gyro values
      gyro_x -= gyro_x_cal;                                                
      gyro_y -= gyro_y_cal;                                                
      gyro_z -= gyro_z_cal;                                                
             
      //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 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
      a += 1;
      Serial.println(a);
      //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
      
      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
      Serial.print(" | Angle  = "); Serial.println(angle_pitch_output);
    
     while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
     loop_timer = micros();//Reset the loop timer
      
    }
    
    
    
    
    void setup_mpu_6050_registers(){
      //Activate the MPU-6050
      Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
      Wire.write(0x6B);                                                    //Send the requested starting register
      Wire.write(0x00);                                                    //Set the requested starting register
      Wire.endTransmission();                                             
      //Configure the accelerometer (+/-8g)
      Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
      Wire.write(0x1C);                                                    //Send the requested starting register
      Wire.write(0x10);                                                    //Set the requested starting register
      Wire.endTransmission();                                             
      //Configure the gyro (500dps full scale)
      Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
      Wire.write(0x1B);                                                    //Send the requested starting register
      Wire.write(0x08);                                                    //Set the requested starting register
      Wire.endTransmission();                                             
    }
    
    
    void read_mpu_6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data
      Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
      Wire.write(0x3B);                                                    //Send the requested starting register
      Wire.endTransmission();                                              //End the transmission
      Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050
      while(Wire.available() < 14);                                        //Wait until all the bytes are received
      acc_x = Wire.read()<<8|Wire.read();                                  
      acc_y = Wire.read()<<8|Wire.read();                                  
      acc_z = Wire.read()<<8|Wire.read();                                  
      temp = 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();                                 
    }

  2. #2
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    7,555
    You may what to check out this thread on the MPU-6050: https://forum.pjrc.com/threads/61755...hlight=mpu6050. The forum member was having the same issue.

    Basically the issue boils down to word size issues between the Teensy and Arduino's. Per @KurtE's post
    Always in cases like this, I would wonder things like assumptions about word size and the like:
    Example: gyro_x = (Wire.read() << 8 | Wire.read()) / 131.0 - gyro_x_error;

    Is Gyro supposed to be a signed number? What I read here is I am assuming always a positive value especially since our word size is 32 bits.
    What happens if you change the code like: gyro_x = (int16_t)(Wire.read() << 8 | Wire.read()) / 131.0 - gyro_x_error;
    which affects your function:
    Code:
    void read_mpu_6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data
      Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
      Wire.write(0x3B);                                                    //Send the requested starting register
      Wire.endTransmission();                                              //End the transmission
      Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050
      while(Wire.available() < 14);                                        //Wait until all the bytes are received
      acc_x = Wire.read()<<8|Wire.read();                                  
      acc_y = Wire.read()<<8|Wire.read();                                  
      acc_z = Wire.read()<<8|Wire.read();                                  
      temp = 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();                                 
    }
    which can be simply fixed by adding (int16_t). eg.
    Code:
     acc_x = (int16_t)(Wire.read() << 8 | Wire.read());
    BTW. You didn't mention what Teensy you are using.

  3. #3
    Senior Member+ KurtE's Avatar
    Join Date
    Jan 2014
    Posts
    9,575
    Sorry I have not played around with these...
    But with cases like this, where it works on an Arduino (AVR based) and not right on Teensy...

    One of the first things I would suspect is data types. Where you define things like:
    Code:
    //Declaring some global variables
    int gyro_x, gyro_y, gyro_z;
    long gyro_x_cal, gyro_y_cal, gyro_z_cal;
    boolean set_gyro_angles;
    Which for example on the two board may and likely have different sizes.

    For example: acc_x = Wire.read()<<8|Wire.read();

    Which may or may not work. Depending on things like are these signed values?
    On a Teensy a long will be 32 bits.

    So this will never give a negative number...

    To work properly on multiple platforms you should use variable types which are setup to specific sizes.
    For example in this case probably: int16_t acc_x;
    Which on both platforms will be the same size.

    It these are NOT signed values, then instead: uint16_t

    Also you might check to see if the Adafruit library Adafruit_MPU6050 helps...

    EDIT - Looks like @mjs513 beat me to it
    Last edited by KurtE; 08-22-2020 at 01:09 PM.

  4. #4
    Junior Member
    Join Date
    Aug 2020
    Posts
    12
    Thanks for the help

  5. #5
    Junior Member
    Join Date
    Aug 2020
    Posts
    12
    Quote Originally Posted by mjs513 View Post
    You may what to check out this thread on the MPU-6050: https://forum.pjrc.com/threads/61755...hlight=mpu6050. The forum member was having the same issue.

    Basically the issue boils down to word size issues between the Teensy and Arduino's. Per @KurtE's post


    which affects your function:
    Code:
    void read_mpu_6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data
      Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
      Wire.write(0x3B);                                                    //Send the requested starting register
      Wire.endTransmission();                                              //End the transmission
      Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050
      while(Wire.available() < 14);                                        //Wait until all the bytes are received
      acc_x = Wire.read()<<8|Wire.read();                                  
      acc_y = Wire.read()<<8|Wire.read();                                  
      acc_z = Wire.read()<<8|Wire.read();                                  
      temp = 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();                                 
    }
    which can be simply fixed by adding (int16_t). eg.
    Code:
     acc_x = (int16_t)(Wire.read() << 8 | Wire.read());
    BTW. You didn't mention what Teensy you are using.
    oh sorry, it was a Teensy 3.2

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •