Problem with MPU-6050 Gyro readings

Using Arduino Sensors
1

Hello everyone this is my first post here :slight_smile:
I have some problems with setting up my first complementary filter with a MPU-6050 Gyro/Acc Sensor.
I used some of the Example code for the raw readings but not I'm not realy sure how to get a tilt from the Gyro to implement it in the complementary filter.
I don't even get the gyro running without any filters. Im just trying the classic newAngle = oldAngle + (w*dt). But there I have alredy my first problems.
Where to get the first old angle?
My coding skills are somewhat limited because I'm pretty new to this stuff.
Could you pls give me some advise on how to get the Gyro and the filter running?
If you need more to know, pls ask.
Thank you :slight_smile:

#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;






float dt = 0.04;

//int alpha = tau/(tau+dt);
float alpha = 0.96;

 float sgyrox = 0.0;
 float sgyroy = 0.0;
 float sgyroz = 0.0;
 
 
void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(38400);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
     
   
    
   

}

   
void loop() {

    
    

    
    accelgyro.getAcceleration(&ax, &ay, &az);
    accelgyro.getRotation(&gx, &gy, &gz);
   
   
    //calculate raw accel data in multiplies of g acc
    float axg = ax/16384.0;
    float ayg = ay/16384.0;
    float azg = az/16384.0;
    
    
    //calculate angles of acc axis
   
    float accx = atan(axg/(sqrt((ayg*ayg)+(azg*azg))));
    float accy = atan(ayg/(sqrt((axg*axg)+(azg*azg))));
    float accz = atan(sqrt((axg*axg)*(ayg*ayg))/azg);
    
    //calculate gyro °/s
    float gyrox = gx/131.0;
    float gyroy = gy/131.0;
    float gyroz = gz/131.0;
    
    
    float gyroax = sgyrox+(gyrox*dt);
    float gyroay = sgyroy+(gyroy*dt);
    float gyroaz = sgyroz+(gyroz*dt);
    
    
    
    
     
    //Fuse gyro and acc
   float anglex = (alpha*gyroax)+((1-alpha)*accx);
   float angley = (alpha*gyroay)+((1-alpha)*accy);
   float anglez = (alpha*gyroaz)+((1-alpha)*accz);
   
   //implementing the new angle in to the gyro calculation
   sgyrox = anglex
   sgyroy = angley
   sgyroz = anglez
   
   
   if (anglex == accx) {
     boolean x = true;
      Serial.print(x);
      Serial.print("\t");
   }
   else {
     boolean x = false; 
    Serial.print(x);
    Serial.print("\t");}
   
   if (angley == accy) {
     boolean y = true;
     Serial.print(y);
     Serial.print("\t");
     }
   else {
     boolean y = false;
      Serial.print(y);
     Serial.print("\t");
   }
   
   Serial.print(gyroax);
   Serial.print("\t");
   Serial.print(gyroay);
   Serial.print("\t");
   Serial.print(accx);
   Serial.print("\t");
   Serial.print(accy);
   
   Serial.println(" ");
    
  

   delay(1);
}
2

Vogler:
Where to get the first old angle?

Since the rate gyro gives you relative motion it doesn't matter what you use for the first old angle. I'd use 0.