This thread has been locked.

If you have a related question, please click the "Ask a related question" button in the top right corner. The newly created question will be automatically linked to this question.

Bug in sensorlib



Hello, I found a bug in TivaWare_C_Series-2.0.1.11577/sensorlib/quaternion.c


There are two wrong signs in the function QuaternionMult():

    //
    // Calculate the Y term
    //
    pfQOut[Q_Y]= ((pfQIn2[Q_W] * pfQIn1[Q_Y]) + (pfQIn2[Q_X] * pfQIn1[Q_Z]) - //must be plus
                  (pfQIn2[Q_Y] * pfQIn1[Q_W]) - (pfQIn2[Q_Z] * pfQIn1[Q_X]));

    //
    // Calculate the Z term
    //
    pfQOut[Q_Z] = ((pfQIn2[Q_W] * pfQIn1[Q_Z]) - (pfQIn2[Q_X] * pfQIn1[Q_Y]) - //<-must be plus
                   (pfQIn2[Q_Y] * pfQIn1[Q_X]) + (pfQIn2[Q_Z] * pfQIn1[Q_W]));

so the right code is:

void
QuaternionMult(float pfQOut[4], float pfQIn1[4], float pfQIn2[4])
{
    //
    // Let Q1 and Q2 be two quaternions with components w,x,y,z
    // Let Qp be the cross product Q1 x Q2.  The components of Qp can be
    // calculated as follows:
    //
    // Qp.w = (Q1w Q2w) - (Q1x Q2x) - (Q1y Q2y) - (Q1z Q2z)
    // Qp.x = (Q1w Q2x) + (Q1x Q2w) - (Q1z Q2y) + (Q1y Q2z)
    // Qp.y = (Q1y Q2w) + (Q1z Q2x) + (Q1w Q2y) - (Q1x Q2z)
    // Qp.z = (Q1z Q2w) - (Q1y Q2x) + (Q1x Q2y) + (Q1w Q2z)
    //

    //
    // Calculate the W term
    //
    pfQOut[Q_W] = ((pfQIn2[Q_W] * pfQIn1[Q_W]) - (pfQIn2[Q_X] * pfQIn1[Q_X]) -
                   (pfQIn2[Q_Y] * pfQIn1[Q_Y]) - (pfQIn2[Q_Z] * pfQIn1[Q_Z]));

    //
    // Calculate the X term
    //
    pfQOut[Q_X]= ((pfQIn2[Q_X] * pfQIn1[Q_W]) + (pfQIn2[Q_W] * pfQIn1[Q_X]) -
                  (pfQIn2[Q_Y] * pfQIn1[Q_Z]) + (pfQIn2[Q_Z] * pfQIn1[Q_Y]));

    //
    // Calculate the Y term
    //
    pfQOut[Q_Y]= ((pfQIn2[Q_W] * pfQIn1[Q_Y]) + (pfQIn2[Q_X] * pfQIn1[Q_Z]) +
                  (pfQIn2[Q_Y] * pfQIn1[Q_W]) - (pfQIn2[Q_Z] * pfQIn1[Q_X]));

    //
    // Calculate the Z term
    //
    pfQOut[Q_Z] = ((pfQIn2[Q_W] * pfQIn1[Q_Z]) - (pfQIn2[Q_X] * pfQIn1[Q_Y]) +
                   (pfQIn2[Q_Y] * pfQIn1[Q_X]) + (pfQIn2[Q_Z] * pfQIn1[Q_W]));
}

  • Thanks Evgeny,


    I've been using this code for a few weeks, but hadn't noticed.  I did recall, however, a post I read a few months ago on another forum (I think it was the Invensense forum), about a bug in TI's quaternion code, but had forgotten about it until now.  I've put your changes, plus another bug I found (will post that separately) in my git repository for the Stellaris library.


    EDIT: It's interesting to note that the equation in comment above this quaternion bug IS correct, just the implementation is in error.

    Cheers,

    Doug