10 years, 11 months ago.

Quadrocopter yaw, pitch, roll calculation from 10-DOF sensor

Hello,

I'm trying to build an autonomous quadrocopter, but I'm having some trouble with calculating the yaw, pitch and roll angles from a 10-DOF imu sensor (3-DOF Gyro, 3-DOF accelerometer, 3-DOF magnetometer and a barometer) with my mbed (LPC1768). IMU Link: http://www.dfrobot.com/index.php?route=product/product&product_id=818

I have found some code on mbed.org for a 10-DOF imu sensor wich has basically the same hardware: http://mbed.org/users/pommzorz/code/HK10DOF/ This code uses quaternions to calculate the yaw, pitch and roll.

The code seems to be working fine, except that the calculated yaw angle from the <<HK10DOF::getYawPitchRoll()>> method seems to return wrong data. The yaw angle value varies between 90-150 degrees while I am turning it 360 degrees around (and holding it horizontally), I compared this data to the compass data with the <<getHeadingXY()>> method. This method returns seemingly correct data. But the data from this method is only reliable when the compas is being held horizontally (cause it doesn't take the Z axis into account). and obviously this isn't always the case in a quadrocopter. So thats why i need the quaternion calculations to work correctly... I've searched the web for some other algorithms that should calculate the yaw correctly from quaternions, but for some reason I am not getting the right results.

Has anybody got any tips or code that i could try? Or does anybody know what could be wrong with the calculations?

this is the code from the HK10DOF source code that should calculate the yaw pitch and roll values from the quaternion:

void HK10DOF::getYawPitchRollRad(float * ypr)
{
    float q[4]; // quaternion
    float gx, gy, gz; // estimated gravity direction
    getQ(q);

    gx = 2 * (q[1]*q[3] - q[0]*q[2]);
    gy = 2 * (q[0]*q[1] + q[2]*q[3]);
    gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];

    ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
    ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
    ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
}