Quaternion calculation help

[deepfreeze]

Hello guys,
I am calculating quaternions from MPU6050 and it works mostly when i move it to roll and pitch but for round movement it drags out i think.
Sorry for my bad explanation but i am attaching a video and my code. Can you check it please ?

void GyroSignals() {

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

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

    Wire.requestFrom(MPU_ADDRESS, 6);

    Gyro_X_Raw = (float) (Wire.read() << 8 | Wire.read());
    Gyro_Y_Raw = (float) (Wire.read() << 8 | Wire.read());
    Gyro_Z_Raw = (float) (Wire.read() << 8 | Wire.read());


    Rate_Gyro_X = (float) Gyro_X_Raw / GYRO_LSB_SENS ; //degree/
    Rate_Gyro_Y = (float) Gyro_Y_Raw / GYRO_LSB_SENS;
    Rate_Gyro_Z = (float) Gyro_Z_Raw / GYRO_LSB_SENS;

}

void setGyroValues(){

    mpu_real_time = micros();
    mpu_Ts = (mpu_real_time - mpu_last_time) / 1000000;

    Rate_Gyro_X -= Rate_Gyro_X_Calib_Err ; //degree/
    Rate_Gyro_Y -= Rate_Gyro_Y_Calib_Err ; //degree/
    Rate_Gyro_Z -= Rate_Gyro_Z_Calib_Err ; //degree/
    data.Gyro_Sens = GYRO_LSB_SENS;

    if(mpu_Ts>0.001){

        Quaternion_dx += Rate_Gyro_X * mpu_Ts;
        Quaternion_dy += Rate_Gyro_Y * mpu_Ts;
        Quaternion_dz += Rate_Gyro_Z * mpu_Ts;

        
        Quaternion_ddx = Quaternion_dx * DEG_TO_RAD;
        Quaternion_ddy = Quaternion_dy * DEG_TO_RAD;
        Quaternion_ddz = Quaternion_dz * DEG_TO_RAD;

        getQuatRotn(Quaternion_ddy, Quaternion_ddz, -Quaternion_ddx);

        Quaternion_dx = Quaternion_dy = Quaternion_dz = 0;

        mpu_last_time = mpu_real_time;

    }
    
}

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

    const double rad2deg = 180.0/3.141592653589793238463;

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


    //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];//w
    Quat[2] += QuatDiff[2];//x
    Quat[3] += QuatDiff[3];//y
    Quat[4] += QuatDiff[4];//z

    //re-normalize
    double quatLen = pow( 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] = a + 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]) * rad2deg;
    Quat_Yaw_Y = asin(-1*Rotn3[1]) * rad2deg;
    Quat_Roll_Z = atan2(Rotn2[1], Rotn1[1]) * rad2deg;
    
}

 

[cls]

The function is

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

But you call it with

getQuatRotn(Quaternion_ddy, Quaternion_ddz, -Quaternion_ddx);

Is that correct?

 

[deepfreeze]

Yes this is the order i call my functions. So first i call GyroSignals and then setGyroValues. Loop them all together. The last 2 functions not important. Just setting the values.

void getMPUData(){

    GyroSignals();
    setGyroValues();
    readAccelData();//not important
    setDataValues(); // not important

}

 

[cls]

Hmm, not what I said. Let's try again.

The parameters of the function call so not match its declaration.

The code declares params x, y, z:

getQuatRotn(dx, day, dz);

But the call is y, z, -x, which is not x, y, z above:

getQuatRotn(ddy, ddz, -ddx);

 

[Grog6]

That's what jumped out at me; wrong calling order.

 

[deepfreeze]

We

Hmm, not what I said. Let's try again.

The parameters of the function call so not match its declaration.

The code declares params x, y, z:

getQuatRotn(dx, day, dz);

But the call is y, z, -x, which is not x, y, z above:

getQuatRotn(ddy, ddz, -ddx);

oh yes I’m sorry about that. The order is different because the orientation of the mpu6050 is different as you can see in the video. It is not vertical to the ground plane.

 

[robopup]

Cool stuff!

I'm not going to debug your code, but if it helps, I'm attaching the general (if I remember correctly?) quaternion class I wrote, and tilt angle class (only to determine angle off vertical, not 3 axis) which calls it. It's loosely based on Keith's excellent blog article:
https://keithp.com/blogs/Quaternions/
Results for all the test flights I've done look qualitatively correct compared to video. And maybe you'll find a bug in my stuff! EDIT: Already found one silly thing I was doing which could be sped up, reattaching - thanks for getting me to look at it!

 

[OverTheTop]

I'm not going to debug your code, but if it helps, I'm attaching the general (if I remember correctly?) quaternion class I wrote, and tilt angle class (only to determine angle off vertical, not 3 axis) which calls it. It's loosely based on Keith's excellent blog article:

Excellent. Thanks. I'm bookmarking this so I can find it later .

 

[waltr]

We

oh yes I’m sorry about that. The order is different because the orientation of the mpu6050 is different as you can see in the video. It is not vertical to the ground plane.

Then it is much better, less confusing, to re-order the Axis names and direction in the 'read sensor' function.