Can't calculate yaw pitch and raw on vertically aligned MPU6050

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
I am using MPU6050 to calculate pitch and roll. My code is working perfect but only if it is a drone. But i want to place my pcb vertically.
Here is how it is now.

thumbnail_IMG_5408.jpg

But i want to use it like this.


thumbnail_IMG_5409.jpg


I am sharing my code. I'll appreciate it if someone can help me.


C++:
void velocitySignals() {

    Wire.beginTransmission(0x68);
    Wire.write(0x1A); //LOW PASS FILTER
    Wire.write(0x05);
    Wire.endTransmission();

    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x43);
    Wire.endTransmission(false);

    Wire.requestFrom(MPU_ADDRESS, 6);

    Gyro_Velocity_X = Wire.read() << 8 | Wire.read();
    Gyro_Velocity_Y = Wire.read() << 8 | Wire.read();
    Gyro_Velocity_Z = Wire.read() << 8 | Wire.read();

    Rate_Roll = (float) Gyro_Velocity_X / GYRO_LSB_SENS; //degree/
    Rate_Pitch = (float) Gyro_Velocity_Y / GYRO_LSB_SENS;
    Rate_Yaw = (float) Gyro_Velocity_Z / GYRO_LSB_SENS;

}

void accelSignals(){
    Wire.beginTransmission(0x68);
    Wire.write(0x1A); //LOW PASS FILTER
    Wire.write(0x05);
    Wire.endTransmission();

    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x3B);
    Wire.endTransmission();

    Wire.requestFrom(MPU_ADDRESS, 6);
    Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Y_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Z_Raw = Wire.read() << 8 | Wire.read();

    //convert data to G unite 1g = 9.8 m/s^2
    Gyro_Accel_X = (float) Gyro_Accel_X_Raw / LSB_SENS - 0.02;
    Gyro_Accel_Y = (float) Gyro_Accel_Y_Raw / LSB_SENS -0.02;
    Gyro_Accel_Z = (float) Gyro_Accel_Z_Raw / LSB_SENS + 0.04;

    Angle_Roll=
    atan(Gyro_Accel_Y/sqrt(pow(Gyro_Accel_X,2)+pow(Gyro_Accel_Z,2)))*1/(3.142/180);
    Angle_Pitch=-
    atan(Gyro_Accel_X/sqrt(pow(Gyro_Accel_Y,2)+pow(Gyro_Accel_Z,2)))*1/(3.142/180);

 }

void gyroDataFusion() {

     kalman_1d(Kalman_Angle_Roll,Kalman_Uncertainty_Angle_Roll,Rate_Roll,Angle_Roll);
     Kalman_Angle_Roll=Kalman1D_Output[0];
     Kalman_Uncertainty_Angle_Roll=Kalman1D_Output[1];

     kalman_1d(Kalman_Angle_Pitch, Kalman_Uncertainty_Angle_Pitch, Rate_Pitch,
     Angle_Pitch);
     Kalman_Angle_Pitch=Kalman1D_Output[0];
     Kalman_Uncertainty_Angle_Pitch=Kalman1D_Output[1];



 }
 

waltr

Well-Known Member
Joined
Jul 17, 2021
Messages
1,182
Reaction score
696
Location
SE Pennsylvania
It is simply a translation of the axis's assigned.

I can't see your board well enough to see the Axis marks. But if same as the AdaFruit MPU6050 I use. Check the Axis's markings to translate and/or the board data sheet.
When flat (drone orientation):
Up = +Z
Right = +Y
Forward (up in picture) = -X
In Rocket orientation:
Up = -X
Right = +Y
half right = -Z (away from view)

Two ways in code to do this:
1- Read the sensor data into the correct variable. Instead of
Code:
    Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Y_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Z_Raw = Wire.read() << 8 | Wire.read();
Do
Code:
    Gyro_Accel_Z_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Y_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();

You may need to negate a value for correct orientation.
Print out to the serial port the Calculated values both ways, write down the values you get with current code in Drone orientation. Then make code changes and check that you get the same values in rocket orientation.

2- translate after reading but before used in calculations.
Like this:
Code:
    Gyro_Accel_Z = (float) Gyro_Accel_X_Raw / LSB_SENS - 0.02;
    Gyro_Accel_Y = (float) Gyro_Accel_Y_Raw / LSB_SENS -0.02;
    Gyro_Accel_X = -(float) Gyro_Accel_Z_Raw / LSB_SENS + 0.04;

Translation must also be done with the Gyro data.

Question:
What are you actually reading in this line?
Code:
Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();
The variable name is Gyro_Accel but can only be one or the other.
Context looks like Accel data so why have Gyro in the name?

Now there is a HUGE PROBLEM using that Kalman filter. It works fine for a Robot or Drone since both can use the Earth's Gravity as a direction reference.
However, a rocket in flight has NO Earth direction reference and the Kalman will give erroneous results.
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
+1 for Waltr. My quick glance at your code I see the same things. First, you cannot use the 1G earth's reference since a rocket is in free-fall. Second, I don't see any code that uses quaternions or quaternion approximations, which are necessary to turn gyro data to rotation data in rockets.
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
It is simply a translation of the axis's assigned.

I can't see your board well enough to see the Axis marks. But if same as the AdaFruit MPU6050 I use. Check the Axis's markings to translate and/or the board data sheet.
When flat (drone orientation):
Up = +Z
Right = +Y
Forward (up in picture) = -X
In Rocket orientation:
Up = -X
Right = +Y
half right = -Z (away from view)

Two ways in code to do this:
1- Read the sensor data into the correct variable. Instead of
Code:
    Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Y_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Z_Raw = Wire.read() << 8 | Wire.read();
Do
Code:
    Gyro_Accel_Z_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_Y_Raw = Wire.read() << 8 | Wire.read();
    Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();

You may need to negate a value for correct orientation.
Print out to the serial port the Calculated values both ways, write down the values you get with current code in Drone orientation. Then make code changes and check that you get the same values in rocket orientation.

2- translate after reading but before used in calculations.
Like this:
Code:
    Gyro_Accel_Z = (float) Gyro_Accel_X_Raw / LSB_SENS - 0.02;
    Gyro_Accel_Y = (float) Gyro_Accel_Y_Raw / LSB_SENS -0.02;
    Gyro_Accel_X = -(float) Gyro_Accel_Z_Raw / LSB_SENS + 0.04;

Translation must also be done with the Gyro data.

Question:
What are you actually reading in this line?
Code:
Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();
The variable name is Gyro_Accel but can only be one or the other.
Context looks like Accel data so why have Gyro in the name?

Now there is a HUGE PROBLEM using that Kalman filter. It works fine for a Robot or Drone since both can use the Earth's Gravity as a direction reference.
However, a rocket in flight has NO Earth direction reference and the Kalman will give erroneous results.
Sorry about the naming. I was thinking it like Gyroscope's Accel Data. But you're right. I changed it. Ill share it now again.
Btw i did it but it seems not working actually. Also yes it is Adafruit MPU6050. Can you look at the code and tell me what to do again ?
Also Should i remove the kalman? and use which values ?

Code:
void GyroSignals() {

    Wire.beginTransmission(0x68);
    Wire.write(0x1A); //LOW PASS FILTER
    Wire.write(0x05);
    Wire.endTransmission();

    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x43);
    Wire.endTransmission(false);

    Wire.requestFrom(MPU_ADDRESS, 6);

    Gyro_X = Wire.read() << 8 | Wire.read();
    Gyro_Y = Wire.read() << 8 | Wire.read();
    Gyro_Z = Wire.read() << 8 | Wire.read();

    Rate_Roll = (float) Gyro_X / GYRO_LSB_SENS; //degree/
    Rate_Pitch = (float) Gyro_Y / GYRO_LSB_SENS;
    Rate_Yaw = (float) Gyro_Z / GYRO_LSB_SENS;

}

void accelSignals(){
    Wire.beginTransmission(0x68);
    Wire.write(0x1A); //LOW PASS FILTER
    Wire.write(0x05);
    Wire.endTransmission();

    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x3B);
    Wire.endTransmission();

    Wire.requestFrom(MPU_ADDRESS, 6);
    Accel_X_Raw = Wire.read() << 8 | Wire.read();
    Accel_Y_Raw = Wire.read() << 8 | Wire.read();
    Accel_Z_Raw = Wire.read() << 8 | Wire.read();

    //convert data to G unite 1g = 9.8 m/s^2
    Accel_X = (float) Accel_X_Raw / LSB_SENS - 0.02;
    Accel_Y = (float) Accel_Y_Raw / LSB_SENS -0.02;
    Accel_Z = (float) Accel_Z_Raw / LSB_SENS + 0.04;


    Angle_Roll=atan(Accel_Y/sqrt(pow(Accel_X,2)+pow(Accel_Z,2)))*1/(3.142/180);
    Angle_Pitch=-atan(Accel_X/sqrt(pow(Accel_Y,2)+pow(Accel_Z,2)))*1/(3.142/180);

}
void mpuDataFusion() {

  kalman_1d(Kalman_Angle_Roll,Kalman_Uncertainty_Angle_Roll,Rate_Roll,Angle_Roll);
  Kalman_Angle_Roll=Kalman1D_Output[0];
  Kalman_Uncertainty_Angle_Roll=Kalman1D_Output[1];

  kalman_1d(Kalman_Angle_Pitch, Kalman_Uncertainty_Angle_Pitch, Rate_Pitch, Angle_Pitch);
  Kalman_Angle_Pitch=Kalman1D_Output[0];
  Kalman_Uncertainty_Angle_Pitch=Kalman1D_Output[1];

 

}
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
+1 for Waltr. My quick glance at your code I see the same things. First, you cannot use the 1G earth's reference since a rocket is in free-fall. Second, I don't see any code that uses quaternions or quaternion approximations, which are necessary to turn gyro data to rotation data in rockets.
How can i calculate Quaternions. I think DMP has something for it but i thought dmp is not good for rockets.
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
How can i calculate Quaternions. I think DMP has something for it but i thought dmp is not good for rockets.

Any DMP that needs a 1G reference isn't good for rockets. There are lots of professionals on the forum that can do it better than me, but take a look at how I do it in my system:


Basically, call getQuatRotn() about 100 times per second and feed it the partial rotations between the steps. You can see how I calculate the partial rotations in the main code file. Let me know if you can't find the right source code
 

tHoagland

Rocket Builder
TRF Supporter
Joined
Oct 25, 2016
Messages
231
Reaction score
78
@SparkyVTFlyer, correct me if I'm wrong, but I seem to recall thay one of the advantages of using quanternions for these calculations is that you avoid the discontinuity points that you gave to deal with in a raw/pitch/yaw space. That's important when feeding values into a Kalman filter.
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
Its a lot of code, so on second thought I'll just post it:

Partial Rotation

C:
//caluclate the partial rotation
dx += gyro.x * fltTime.dt;
dy += gyro.y * fltTime.dt;
dz += gyro.z * fltTime.dt;

//update the quaternion rotation
if(fltTime.timeCurrent - lastRotn > rotnRate){
      
    const float rotn2rad = gyro.gain * (3.14159265359 / 180) / 1000000;
    ddx = dx * rotn2rad;
    ddy = dy * rotn2rad;
    ddz = dz * rotn2rad;
      
    getQuatRotn(ddx, ddy, ddz);
    dx = dy = dz = 0L;
      
    lastRotn = fltTime.timeCurrent;}

Quaternion Differential

C:
void getQuatRotn(float dx, float dy, float dz){

  //Local Vectors
  float QuatDiff[5];
  float Rotn1[4];
  float Rotn2[4];
  float Rotn3[4];
 
  //Local rotation holders
  static long prevRollZ = 0;
  static long quatRollZ = 0;
  static long fullRollZ = 0;
 
  //Compute quaternion derivative
  QuatDiff[1] = 0.5 * (-1 * dx * Quat[2] - dy * Quat[3] - dz * Quat[4]);
  QuatDiff[2] = 0.5 * (     dx * Quat[1] - dy * Quat[4] + dz * Quat[3]);
  QuatDiff[3] = 0.5 * (     dx * Quat[4] + dy * Quat[1] - dz * Quat[2]);
  QuatDiff[4] = 0.5 * (-1 * dx * Quat[3] + dy * Quat[2] + dz * Quat[1]);
 
  //Update the quaternion
  Quat[1] += QuatDiff[1];
  Quat[2] += QuatDiff[2];
  Quat[3] += QuatDiff[3];
  Quat[4] += QuatDiff[4];
 
  //re-normalize
  float quatLen = powf( Quat[1]*Quat[1] + Quat[2]*Quat[2] + Quat[3]*Quat[3] + Quat[4]*Quat[4], -0.5);
  Quat[1] *= quatLen;
  Quat[2] *= quatLen;
  Quat[3] *= quatLen;
  Quat[4] *= quatLen;
      
  //compute the components of the rotation matrix
  float a = Quat[1];
  float b = Quat[2];
  float c = Quat[3];
  float d = Quat[4];
  float a2 = a*a;
  float b2 = b*b;
  float c2 = c*c;
  float d2 = d*d;
  float ab = a*b;
  float ac = a*c;
  float ad = a*d;
  float bc = b*c;
  float bd = b*d;
  float cd = c*d;
      
  //Compute rotation matrix
  Rotn1[1] = a2 + b2 - c2 - d2;
  //Rotn1[2] = 2 * (bc - ad);
  //Rotn1[3] = 2 * (bd + ac);
  Rotn2[1] = 2 * (bc + ad);
  //Rotn2[2] = a2 - b2 + c2 - d2;
  //Rotn2[3] = 2 * (cd - ab);
  Rotn3[1] = 2 * (bd - ac);
  Rotn3[2] = 2 * (cd + ab);
  Rotn3[3] = a2 - b2 - c2 + d2;
 
  //compute 3D orientation
  pitchX = speedAtan2(Rotn3[2], Rotn3[3]);
  yawY = speedArcSin(-1*Rotn3[1]);
 
  prevRollZ = quatRollZ;
  quatRollZ = speedAtan2(Rotn2[1], Rotn1[1]);
  if(quatRollZ - prevRollZ > 1800){fullRollZ--;}
  else if(quatRollZ - prevRollZ < -1800){fullRollZ++;}
  rollZ = (fullRollZ*3600 + quatRollZ)*.1;
 
  //Compute angle off vertical
  float tanYaw = speedTan(yawY);
  float tanPitch = speedTan(pitchX);
 
  float hyp1 = tanYaw*tanYaw + tanPitch*tanPitch;
  float hyp2 = powf(hyp1, 0.5);
  offVert = speedArcTan(hyp2);
 
  //as long as the rocket never exceeded 45 degrees, set the rotnOK variable to whether or not the current off-vertical rotation is within the allowable cone
  rotnOK = (!rotationFault && offVert < settings.maxAngle) ? true : false;
 
  //if the rocket goes outside a 45 degree cone, then permanently shut-down all airstart events.  The rocket is probably skywriting or experienced a failure
  if (!rotationFault && offVert > 450){rotationFault = true; rotnOK = false;}
 
}//end getQuatRotn

I've got some tricky methods to quickly do trig functions, but you can replace mine with floating point C trig functions.
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
@SparkyVTFlyer, correct me if I'm wrong, but I seem to recall thay one of the advantages of using quanternions for these calculations is that you avoid the discontinuity points that you gave to deal with in a raw/pitch/yaw space. That's important when feeding values into a Kalman filter.

Yes, quaternions avoid the gimbal-lock situations that are frequently a problem with Euler methods. However, in a rocket if you are only interested in off-vertical rotations less than 90 degrees, which is almost always the case for us, then you can use Euler methods or thier approximations. In my code, I have a routine called DCM2D (Direct Cosine Method in 2-Dimensions) that is this approximation. It gives me the exact same roll and off-vertical values as quaternions, but requires an order of magnitude less floating point calculations. Its useful when you don't have a processor with an Floating Point Unit (FPU).
 

tHoagland

Rocket Builder
TRF Supporter
Joined
Oct 25, 2016
Messages
231
Reaction score
78
Yes, quaternions avoid the gimbal-lock situations that are frequently a problem with Euler methods. However, in a rocket if you are only interested in off-vertical rotations less than 90 degrees, which is almost always the case for us, then you can use Euler methods or thier approximations. In my code, I have a routine called DCM2D (Direct Cosine Method in 2-Dimensions) that is this approximation. It gives me the exact same roll and off-vertical values as quaternions, but requires an order of magnitude less floating point calculations. Its useful when you don't have a processor with an Floating Point Unit (FPU).
Thanks for the insight. I've always just saved logged data and done the attitude calculations in post processing. It's great to get some insight into the real-world considerations for doing those calculations onboard.
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
Yes, quaternions avoid the gimbal-lock situations that are frequently a problem with Euler methods. However, in a rocket if you are only interested in off-vertical rotations less than 90 degrees, which is almost always the case for us, then you can use Euler methods or thier approximations. In my code, I have a routine called DCM2D (Direct Cosine Method in 2-Dimensions) that is this approximation. It gives me the exact same roll and off-vertical values as quaternions, but requires an order of magnitude less floating point calculations. Its useful when you don't have a processor with an Floating Point Unit (FPU).
I am usin Teensy 4.0 as microcontroller. I believe ARM Cortex-M7 also has FPU. So what should i do ?
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
Let me see if i understood it correctly. Sorry for my stupidness guys. I appreciate your help.

So these quternions are a different representation of the position of the obejct right? Also they dont use the accelerometer values and we don't want to use them because rocket is flying and falling free. So when we calculate angle from accellerometer as Euler angles. It gives us the problem as Gimble Lock. Good.
So i get rid of the Kalman filter that i feed with the accelerometer Euler data calculation which is like this;
Code:
    Angle_Roll=atan(Accel_Y/sqrt(pow(Accel_X,2)+pow(Accel_Z,2)))*1/(3.142/180);
    Angle_Pitch=-atan(Accel_X/sqrt(pow(Accel_Y,2)+pow(Accel_Z,2)))*1/(3.142/180);

So instead of that, i will use the Gyro calues with the quternion thing and try to calculate the position of the rocket. Is it right ?
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
Let me see if i understood it correctly. Sorry for my stupidness guys. I appreciate your help.

So these quternions are a different representation of the position of the obejct right? Also they dont use the accelerometer values and we don't want to use them because rocket is flying and falling free. So when we calculate angle from accellerometer as Euler angles. It gives us the problem as Gimble Lock. Good.
So i get rid of the Kalman filter that i feed with the accelerometer Euler data calculation which is like this;
Code:
    Angle_Roll=atan(Accel_Y/sqrt(pow(Accel_X,2)+pow(Accel_Z,2)))*1/(3.142/180);
    Angle_Pitch=-atan(Accel_X/sqrt(pow(Accel_Y,2)+pow(Accel_Z,2)))*1/(3.142/180);

So instead of that, i will use the Gyro calues with the quternion thing and try to calculate the position of the rocket. Is it right ?

Other folks may tell you different, but I recommend you only use the gyro outputs with quaternions to determine the rotational attitude of the rocket. It will be important to calibrate the gyro so that the drift isn't too bad. You can determine the gyro offsets by sampling the gyro when its perfectly still. The average values that it outputs are your offsets.
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
I calibrate my gyro such as;
Getting values for 2000 times (Calibration_Count = 2000) and fix with the error when i am getting the actual values.

Code:
void calibrateGyro(){
 
  for(int i=0;i<Calibration_Count;i++){
    GyroSignals();

    Rate_Roll_Calib_Err += Rate_Roll;
    Rate_Pitch_Calib_Err += Rate_Pitch;
    Rate_Yaw_Calib_Err += Rate_Yaw;
    delay(1);
  }

  Rate_Roll_Calib_Err /= Calibration_Count;
  Rate_Pitch_Calib_Err /= Calibration_Count;
  Rate_Yaw_Calib_Err /= Calibration_Count;


}
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
Looks about right. What is the output data rate you set the MPU6050 to? Since I dont see any delay statements in your code I suspect you are probably sampling the gyro faster than it is outputting data. This means that you think you might be getting 2000 samples, but in reality you are getting much less since many of those are repeated.
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
After the long conversation with Sparky. I implemented the code but this is my result. I am doing wrong somewhere but i don't know what.
 

Attachments

  • WhatsApp Video 2022-12-31 at 23.50.09.mp4
    1.3 MB

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
Here is my full code

Code:
double Quat_Pitch_X, Quat_Yaw_Y, Quat_Roll_Z;
double Quaternion_dx,Quaternion_dy,Quaternion_dz;
double Quat[5] = {0.0, 1.0, 0.0, 0.0, 0.0};

void getMPUData(){
    readGyroData();
    readAccelData();
    setDataValues();
    //mpuDataFusion(); Kalman thing i'll get rid of it.
}

void readGyroData(){
  GyroSignals();
  setGyroValues();
}
void GyroSignals() {
    


    Wire.beginTransmission(0x68);
    Wire.write(0x1A); //LOW PASS FILTER
    Wire.write(0x05);
    Wire.endTransmission();

    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x43);
    Wire.endTransmission(false);

    Wire.requestFrom(MPU_ADDRESS, 6);

    Gyro_X = Wire.read() << 8 | Wire.read();
    Gyro_Y = Wire.read() << 8 | Wire.read();
    Gyro_Z = Wire.read() << 8 | Wire.read();

    Rate_Roll = (float) Gyro_X / GYRO_LSB_SENS ; //degree/
    Rate_Pitch = (float) Gyro_Y / GYRO_LSB_SENS;
    Rate_Yaw = (float) Gyro_Z / GYRO_LSB_SENS;

    Rate_Roll -= Rate_Roll_Calib_Err ; //degree/
    Rate_Pitch -= Rate_Pitch_Calib_Err ; //degree/
    Rate_Yaw -= Rate_Yaw_Calib_Err ; //degree/
}
void setGyroValues(){

    mpu_last_time = mpu_real_time;
    mpu_real_time = millis();
    mpu_Ts = (mpu_real_time - mpu_last_time) / 1000;

    Quaternion_dx = Rate_Roll * mpu_Ts;
    Quaternion_dy = Rate_Pitch * mpu_Ts;
    Quaternion_dz = Rate_Yaw * mpu_Ts;

    
    getQuatRotn(Quaternion_dx, Quaternion_dy, Quaternion_dz);


}

void getQuatRotn(double dx, double dy, double dz){

    //Local Vectors
    double QuatDiff[5];
    double Rotn1[4];
    double Rotn2[4];
    double Rotn3[4];

    //Local rotation holders
    static long prevRollZ = 0;
    static long quatRollZ = 0;
    static long fullRollZ = 0;

    //Compute quaternion derivative
    QuatDiff[1] = 0.5 * (-1 * dx * Quat[2] - dy * Quat[3] - dz * Quat[4]);
    QuatDiff[2] = 0.5 * (     dx * Quat[1] - dy * Quat[4] + dz * Quat[3]);
    QuatDiff[3] = 0.5 * (     dx * Quat[4] + dy * Quat[1] - dz * Quat[2]);
    QuatDiff[4] = 0.5 * (-1 * dx * Quat[3] + dy * Quat[2] + dz * Quat[1]);

    //Update the quaternion
    Quat[1] += QuatDiff[1];
    Quat[2] += QuatDiff[2];
    Quat[3] += QuatDiff[3];
    Quat[4] += QuatDiff[4];

    //re-normalize
    double quatLen = powf( Quat[1]*Quat[1] + Quat[2]*Quat[2] + Quat[3]*Quat[3] + Quat[4]*Quat[4], -0.5);
    Quat[1] *= quatLen;
    Quat[2] *= quatLen;
    Quat[3] *= quatLen;
    Quat[4] *= quatLen;
        
    //compute the components of the rotation matrix
    double a = Quat[1];
    double b = Quat[2];
    double c = Quat[3];
    double d = Quat[4];
    double a2 = a*a;
    double b2 = b*b;
    double c2 = c*c;
    double d2 = d*d;
    double ab = a*b;
    double ac = a*c;
    double ad = a*d;
    double bc = b*c;
    double bd = b*d;
    double cd = c*d;
        
    //Compute rotation matrix
    Rotn1[1] = a2 + b2 - c2 - d2;
    //Rotn1[2] = 2 * (bc - ad);
    //Rotn1[3] = 2 * (bd + ac);
    Rotn2[1] = 2 * (bc + ad);
    //Rotn2[2] = a2 - b2 + c2 - d2;
    //Rotn2[3] = 2 * (cd - ab);
    Rotn3[1] = 2 * (bd - ac);
    Rotn3[2] = 2 * (cd + ab);
    Rotn3[3] = a2 - b2 - c2 + d2;

    //compute 3D orientation
    Quat_Pitch_X = atan2(Rotn3[2], Rotn3[3]);
    Quat_Yaw_Y = asin(-1*Rotn3[1]);

    
    prevRollZ = quatRollZ;
    quatRollZ = atan2(Rotn2[1], Rotn1[1]);
    if(quatRollZ - prevRollZ > 1800){fullRollZ--;}
    else if(quatRollZ - prevRollZ < -1800){fullRollZ++;}
    Quat_Roll_Z = (fullRollZ*3600 + quatRollZ)*.1;

    Quat_Yaw_Y = Quat_Yaw_Y  * (180.0/3.141592653589793238463);
    Quat_Pitch_X = Quat_Pitch_X  * (180.0/3.141592653589793238463);
    Quat_Roll_Z = Quat_Roll_Z  * (180.0/3.141592653589793238463);

}
 
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
Here is my full code

Code:
double Quat_Pitch_X, Quat_Yaw_Y, Quat_Roll_Z;
double Quaternion_dx,Quaternion_dy,Quaternion_dz;
double Quat[5] = {0.0, 1.0, 0.0, 0.0, 0.0};

void getMPUData(){
    readGyroData();
    readAccelData();
    setDataValues();
    //mpuDataFusion(); Kalman thing i'll get rid of it.
}

void readGyroData(){
  GyroSignals();
  setGyroValues();
}
void GyroSignals() {
   


    Wire.beginTransmission(0x68);
    Wire.write(0x1A); //LOW PASS FILTER
    Wire.write(0x05);
    Wire.endTransmission();

    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x43);
    Wire.endTransmission(false);

    Wire.requestFrom(MPU_ADDRESS, 6);

    Gyro_X = Wire.read() << 8 | Wire.read();
    Gyro_Y = Wire.read() << 8 | Wire.read();
    Gyro_Z = Wire.read() << 8 | Wire.read();

    Rate_Roll = (float) Gyro_X / GYRO_LSB_SENS ; //degree/
    Rate_Pitch = (float) Gyro_Y / GYRO_LSB_SENS;
    Rate_Yaw = (float) Gyro_Z / GYRO_LSB_SENS;

    Rate_Roll -= Rate_Roll_Calib_Err ; //degree/
    Rate_Pitch -= Rate_Pitch_Calib_Err ; //degree/
    Rate_Yaw -= Rate_Yaw_Calib_Err ; //degree/
}
void setGyroValues(){

    mpu_last_time = mpu_real_time;
    mpu_real_time = millis();
    mpu_Ts = (mpu_real_time - mpu_last_time) / 1000;

    Quaternion_dx = Rate_Roll * mpu_Ts;
    Quaternion_dy = Rate_Pitch * mpu_Ts;
    Quaternion_dz = Rate_Yaw * mpu_Ts;

   
    getQuatRotn(Quaternion_dx, Quaternion_dy, Quaternion_dz);


}

void getQuatRotn(double dx, double dy, double dz){

    //Local Vectors
    double QuatDiff[5];
    double Rotn1[4];
    double Rotn2[4];
    double Rotn3[4];

    //Local rotation holders
    static long prevRollZ = 0;
    static long quatRollZ = 0;
    static long fullRollZ = 0;

    //Compute quaternion derivative
    QuatDiff[1] = 0.5 * (-1 * dx * Quat[2] - dy * Quat[3] - dz * Quat[4]);
    QuatDiff[2] = 0.5 * (     dx * Quat[1] - dy * Quat[4] + dz * Quat[3]);
    QuatDiff[3] = 0.5 * (     dx * Quat[4] + dy * Quat[1] - dz * Quat[2]);
    QuatDiff[4] = 0.5 * (-1 * dx * Quat[3] + dy * Quat[2] + dz * Quat[1]);

    //Update the quaternion
    Quat[1] += QuatDiff[1];
    Quat[2] += QuatDiff[2];
    Quat[3] += QuatDiff[3];
    Quat[4] += QuatDiff[4];

    //re-normalize
    double quatLen = powf( Quat[1]*Quat[1] + Quat[2]*Quat[2] + Quat[3]*Quat[3] + Quat[4]*Quat[4], -0.5);
    Quat[1] *= quatLen;
    Quat[2] *= quatLen;
    Quat[3] *= quatLen;
    Quat[4] *= quatLen;
       
    //compute the components of the rotation matrix
    double a = Quat[1];
    double b = Quat[2];
    double c = Quat[3];
    double d = Quat[4];
    double a2 = a*a;
    double b2 = b*b;
    double c2 = c*c;
    double d2 = d*d;
    double ab = a*b;
    double ac = a*c;
    double ad = a*d;
    double bc = b*c;
    double bd = b*d;
    double cd = c*d;
       
    //Compute rotation matrix
    Rotn1[1] = a2 + b2 - c2 - d2;
    //Rotn1[2] = 2 * (bc - ad);
    //Rotn1[3] = 2 * (bd + ac);
    Rotn2[1] = 2 * (bc + ad);
    //Rotn2[2] = a2 - b2 + c2 - d2;
    //Rotn2[3] = 2 * (cd - ab);
    Rotn3[1] = 2 * (bd - ac);
    Rotn3[2] = 2 * (cd + ab);
    Rotn3[3] = a2 - b2 - c2 + d2;

    //compute 3D orientation
    Quat_Pitch_X = atan2(Rotn3[2], Rotn3[3]);
    Quat_Yaw_Y = asin(-1*Rotn3[1]);

   
    prevRollZ = quatRollZ;
    quatRollZ = atan2(Rotn2[1], Rotn1[1]);
    if(quatRollZ - prevRollZ > 1800){fullRollZ--;}
    else if(quatRollZ - prevRollZ < -1800){fullRollZ++;}
    Quat_Roll_Z = (fullRollZ*3600 + quatRollZ)*.1;

    Quat_Yaw_Y = Quat_Yaw_Y  * (180.0/3.141592653589793238463);
    Quat_Pitch_X = Quat_Pitch_X  * (180.0/3.141592653589793238463);
    Quat_Roll_Z = Quat_Roll_Z  * (180.0/3.141592653589793238463);

}

I sent you a DM. I see a few things in the code that might need fixing. The fact that its giving you nearly stable values in 2 axes makes me think it may be a calibration issue, or a simple sign inversion. I'm also not a big fan of the MPU6050. If you look at the datasheet you'll see that it has huge zero-level offsets, massive temperature sensitivity, and gyro-acceleration cross-axis sensitivity. Not super great for rockets if you are trying to get an accurate estimate of roll-pitch-yaw.

Take a look at the LSM6DSOX. Its the latest/greatest from ST and has much better specs than the MPU6050.
 

waltr

Well-Known Member
Joined
Jul 17, 2021
Messages
1,182
Reaction score
696
Location
SE Pennsylvania
After the long conversation with Sparky. I implemented the code but this is my result. I am doing wrong somewhere but i don't know what.
Is the sensor not moving during this video?
If so then looks like there may be an issue with the Gyro calibration on one axis.
Do you output the Cal values on serial?
Sparky has good code and glad to see him posting help.
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
Is the sensor not moving during this video?
If so then looks like there may be an issue with the Gyro calibration on one axis.
Do you output the Cal values on serial?
Sparky has good code and glad to see him posting help.
Yes his code is working. I am making some mistakes. He is helping me. I achieved to make it stabilized now. I will post here when we figure out. Cz there might be so many mistakes. I don't wanna flood here.
 

deepfreeze

Active Member
TRF Supporter
Joined
Dec 3, 2022
Messages
43
Reaction score
1
Location
New Jersey, US
Hello guys. I finally made it work. My mistake was that i wasn't converting the datas to the radian before i give it to the Sparky's function.
 

Attachments

  • 323889164_561193422233365_5917734614976005677_n.mp4
    3 MB
Joined
Feb 8, 2009
Messages
295
Reaction score
96
Location
Fairfax Station, VA
You’ll need another sensor to periodically correct the drift. However, our rocket flights are usually 1 minute or less. Within that timeframe drift is usually minimal. Drones typically will use the accelerometer to do this when in stable flight, but it won’t work for rockets.
 
Top