Revert "Use QQuaternion instead of mavlink_quaternion_to_euler"

[gillamkid]

Issue

The recent change to use QQuaternion instead of mavlink_quaternion_to_euler introduced an error; gimbal yaw is no longer correctly decoded.

Proof of Issue

I don't understand the difference between mavlink_quaternion_to_euler and QQuaternion::toEulerAngles() , but I ran some tests which show that they cannot be interchanged the way they were. Here is the test that I ran.

// here is a sample set of euler roll,pitch/yaw values we'll convert to a quaternion and then back to euler
float origRollDeg = 0.0f;
float origPitchDeg = 30.0f;
float origYawDeg = 45.0f;
qCritical() << "Original Euler angles - roll: " << origRollDeg
            << " pitch: " << origPitchDeg
            << " yaw: " << origYawDeg;

// convert euler roll,pitch,yaw to quaternion using MAVLink's method
float q[4];
mavlink_euler_to_quaternion(
        qDegreesToRadians(origRollDeg),
        qDegreesToRadians(origPitchDeg),
        qDegreesToRadians(origYawDeg),
        q);
qCritical() << "MAVLink encode result - q[0]: " << q[0]<< " q[1]: " << q[1] << " q[2]: " << q[2] << " q[3]: " << q[3];

// convert quaternion back to Euler angles using MAVLink's method: result is correct/expected
float mavlinkRollRad, mavlinkPitchRad, mavlinkYawRad;
mavlink_quaternion_to_euler(q, &mavlinkRollRad, &mavlinkPitchRad, &mavlinkYawRad);
qCritical() << "MAVLink decode result - roll: " << qRadiansToDegrees(mavlinkRollRad)
            << " pitch: " << qRadiansToDegrees(mavlinkPitchRad)
            << " yaw: " << qRadiansToDegrees(mavlinkYawRad);

// convert quaternion back to Euler angles using QQuaternion: result is incorrect/unexpected
QQuaternion qq(
    q[0],
    q[1],
    q[2],
    q[3]);
auto vector3D = qq.toEulerAngles();
float qqRollDeg = vector3D.z();
float qqPitchDeg = vector3D.y();
float qqYawDeg = vector3D.x();
qCritical() << "QQuaternion decode result - roll: " << qqRollDeg
            << " pitch: " << qqPitchDeg
            << " yaw: " << qqYawDeg;

The output is the following

Original Euler angles - roll:  0  pitch:  30  yaw:  45
MAVLink encode result - q[0]:  0.892399  q[1]:  -0.0990458  q[2]:  0.239118  q[3]:  0.369644
MAVLink decode result - roll:  -2.48753e-07  pitch:  30  yaw:  45
QQuaternion decode result - roll:  40.8934  pitch:  22.2077  yaw:  -20.7048

@DonLakeFlyer , you were the one who made the recent change to use QQuaternion. I suspect it looked fine in your testing because you likely were testing cases where yaw was 0. If I run the same test as above, only chage origYawDeg to be 0, I get the following output:

Original Euler angles - roll:  0  pitch:  30  yaw:  0
MAVLink encode result - q[0]:  0.965926  q[1]:  0  q[2]:  0.258819  q[3]:  0
MAVLink decode result - roll:  0  pitch:  30  yaw:  0
QQuaternion decode result - roll:  0  pitch:  30  yaw:  0

[DonLakeFlyer]

This shouldn't be reverted. The mavlink version has too many problems with NaNs. It should be fixed to be able to use QQuaternion correctly, whatever that is. I'll try to figure it out. I think I only tested pitch changes, so probably caught out by that.

[DonLakeFlyer]

It's Euler-order convention problem:

• mavlink - x,y,z = roll,pitch,yaw
• Qt - x,y,z = yaw,pitch,roll

Which also explains why my pitch only testing worked fine. Sigh...

[DonLakeFlyer]

@gillamkid Can you try changing this and testing with your setup:

    // Convert from QQuaternion to Euler angles. We specifically don't use mavlink_quaternion_to euler
    // because that seems to spew NaNs for boundary conditions. Whereas QQuaternion seems to handle things
    // more cleanly.
    QQuaternion q(
        attitude_status.q[0],
        attitude_status.q[3],
        attitude_status.q[2],
        attitude_status.q[1]);

Note the reversed order to x,y,z

[gillamkid]

@gillamkid Can you try changing this and testing with your setup:

    // Convert from QQuaternion to Euler angles. We specifically don't use mavlink_quaternion_to euler
    // because that seems to spew NaNs for boundary conditions. Whereas QQuaternion seems to handle things
    // more cleanly.
    QQuaternion q(
        attitude_status.q[0],
        attitude_status.q[3],
        attitude_status.q[2],
        attitude_status.q[1]);

Note the reversed order to x,y,z

@DonLakeFlyer closer! But there are still issues

If I ran the same test I did before after applying your suggested fix, the test case passes

Original Euler angles - roll:  0  pitch:  30  yaw:  45
MAVLink encode result - q[0]:  0.892399  q[1]:  -0.0990458  q[2]:  0.239118  q[3]:  0.369644
MAVLink decode result - roll:  -2.48753e-07  pitch:  30  yaw:  45
QQuaternion decode result - roll:  0  pitch:  30  yaw:  45

however, if I test a case where yaw is greater than 90, the test fails

Original Euler angles - roll:  0  pitch:  30  yaw:  105
MAVLink encode result - q[0]:  0.588018  q[1]:  -0.205335  q[2]:  0.157559  q[3]:  0.76632
MAVLink decode result - roll:  1.10538e-06  pitch:  30  yaw:  105
QQuaternion decode result - roll:  180  pitch:  -150  yaw:  75

[DonLakeFlyer]

Argh!

[DonLakeFlyer]

I tracked the NaNs for pitch coming from here: mavlink/mavlink#2389. We'll see...

[DonLakeFlyer]

So for now, we can revert till I figure out what is going on.