MPU6050 ypr simulation shows glitchy result

[deepfreeze]

Hello my fellow friends,
I am so happy that i found a forum to throw my problems and talk to a human. Also everyone is so nice. I love you guys.

Alright after love explosion,

I want to build a model rocket for an F-15 motor and 98mm tube. I am using MPU6050 for that. But as you know DMP is not working good for rocketry.

So i want to get the values manually. But i couldn't find a way to do it. I wrote a code but it gives me the angles wrong. I mean not wrong but considering that i will place my gyro into the rocket vertically, i couldn't figure out how i would do it.
I am sharing you a code and my simulation to show you what i mean. It seems like getting the values. But then it correct itself. It is glitchy. It was good when i was using DMP. I understand it wont be as accurate as DMP but i didnt expect this much. Could you please help me ?
I want to improve the usage of this device.

#include<Wire.h>

#include <ArduinoJson.h>
//CONSTANTS


int MPU_addr = 0x68;// 0x69 when AD0 is HIGH

int MPU_POWER_ADDR = 0X6B;




int16_t AccX,AccY,AccZ,Temp,GyroX,GyroY,GyroZ;


int AFS_SEL_2 = 0x00;
int AFS_SEL_4 = 0x08;
int AFS_SEL_8 = 0x10;
int AFS_SEL_16 = 0x18;

int FS_SEL_2 = 0x00;
int FS_SEL_4 = 0x08;
int FS_SEL_8 = 0x10;
int FS_SEL_16 = 0x18;

int LSB_SENS = 0;
int GYRO_LSB_SENS = 0;


int minVal=265;
int maxVal=402;
 
double x;
double y;
double z;

double accel_x_cal,accel_y_cal,accel_z_cal;
double accelX, accelY, accelZ;

float rollangle,pitchangle;
float roll,pitch,yaw;

double gyroX, gyroY, gyroZ;
double gyro_x_cal,gyro_y_cal,gyro_z_cal;



#define A 0.962
#define dt 0.020


void setAccelSensitivity(int sens){

    Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C); // start device
  Wire.write(sens);
 
  Wire.endTransmission(true);

  if(sens = AFS_SEL_2){
    LSB_SENS =  16384;
   
   }
   if(sens = AFS_SEL_4){
    LSB_SENS =  8192;
   
   }
   if(sens = AFS_SEL_8){
    LSB_SENS =  4096;
   
   }
   if(sens = AFS_SEL_16){
    LSB_SENS =  2048;
   
   }
 
  }

void setGyroSensitivity(int sens){

    Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B); // start device
  Wire.write(sens);
 
  Wire.endTransmission(true);

  if(sens = AFS_SEL_2){
    GYRO_LSB_SENS =  131;
   
   }
   if(sens = AFS_SEL_4){
    GYRO_LSB_SENS =  65.5;
   
   }
   if(sens = AFS_SEL_8){
    GYRO_LSB_SENS =  32.8;
   
   }
   if(sens = AFS_SEL_16){
    GYRO_LSB_SENS =  16.4;
   
   }
 
  }



void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(MPU_POWER_ADDR); // start device
  Wire.write(0);
 

  Wire.endTransmission(true);
  setAccelSensitivity(AFS_SEL_2);
 
  setGyroSensitivity(FS_SEL_16);

  Serial.begin(9600);


}


void loop(){

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU_addr,14,true);

  AccX = Wire.read() << 8 | Wire.read(); //ACCEL_XOUT_H & ACCEL_XOUT_L
  AccY = Wire.read() << 8 | Wire.read(); //ACCEL_YOUT_H & ACCEL_YOUT_L
  AccZ = Wire.read() << 8 | Wire.read(); //ACCEL_ZOUT_H & ACCEL_ZOUT_L
 
  Temp = Wire.read() << 8 | Wire.read(); //TEMP_OUT_H & TEMP_OUT_L
 
  GyroX = Wire.read() << 8 | Wire.read(); //ACCEL_XOUT_H & ACCEL_XOUT_L
  GyroY = Wire.read() << 8 | Wire.read(); //ACCEL_YOUT_H & ACCEL_YOUT_L
  GyroZ = Wire.read() << 8 | Wire.read(); //ACCEL_ZOUT_H & ACCEL_ZOUT_L



  accelX = AccX / LSB_SENS;
  accelY = AccY / LSB_SENS;
  accelZ = AccZ / LSB_SENS;

  gyroX = GyroX / GYRO_LSB_SENS;
  gyroY = GyroY / GYRO_LSB_SENS;
  gyroZ = GyroZ / GYRO_LSB_SENS;

  rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET
 
  //Using Complemetary Filter
  roll=A*(roll+gyroX*dt)+(1-A)*rollangle;
  pitch=A*(pitch+gyroY*dt)+(1-A)*pitchangle;
 
  yaw=gyroZ;
 
        DynamicJsonDocument doc(1024);
        doc["CODE"] = "MPU_DATA";
        doc["yaw"] = yaw;
        doc["pitch"] = pitch;
        doc["roll"] = roll;
        doc["x_accel"] = 0;
        doc["y_accel"] = 0;
        doc["z_accel"] = 0;
       

        serializeJson(doc, Serial);
        Serial.println();

 
 

}

 

[jderimig]

Have you corrected for the gyro zero offsets?

I suggest
- Comment out your initial roll angle and pitch angle calcs and set each to 0.
- Ditch the EMA filter for the roll and pitch integrations, integrating is already a filter.
- Yaw is not gyro_z. In your schema Yaw is Yaw += gyro_z*dt <--[ that is probably responsible for the glitchiness in your video.

Try that to see if the glitchiness goes away. Test it with small rotations. Test it with like 5 degree wobbles and see if you system works.

 

[waltr]

Good suggestions from jderimig.

Looking at your code:
How do you know the Time interval between sample is 20ms??

#define dt 0.020

The loop() has nothing in it to fix the speed.

Gyros DO have offset errors that should be measured with NO MOTION then saved and subtracted for every reading.

Both the offset errors and the wrong dt will affect the results of integrating the gyro data to angle position.

  rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET

This code only works for a device not in a rocket and is fine for the 'demo' code you are currently doing.
This requires the Earth's Gravity vector as reference. In a rocket once it leaves the pad has no Earth's gravity reference.

 

[jderimig]

Good suggestions from jderimig.

Looking at your code:
How do you know the Time interval between sample is 20ms??

#define dt 0.020

Good catch. Read millis() or micros() when you read the gyros and accels and get a timestamp to determine your actual dt at each integration step for better practice (if not using an real time OS or timer interrupts).