SparkFun Forums 

Where electronics enthusiasts find answers.

Have questions about a SparkFun product or board? This is the place to be.
By tedd
#193498
Having a 9DoF Razor IMU M0 (SEN-14001) with the default firmware, enabling Euler angles with the "e" command, one is supposed to get Pitch, Roll and Yaw as the last 3 values. However, by looking at the Pitch value at the same time as the actual movements of the board, the Pitch value is twice as much as the real value.
In other words, if I pitch the board 45 degrees, I read 90 degrees. But when I roll it 45 degrees, the Roll value is correctly 45 degrees. The Yaw value is also correct. 0-360 degrees correspond to a full rotation.

Actually, "Pitch" and "Roll" in this firmware seem to be rather rotation around the X and Y axis given when Yaw=0 degrees, so they aren't really Pitch and Roll relative to the board itself, but to the absolute "forward" direction of the gyros. That's not a big problem, although it would have been more intuitive to have Roll and Pitch relative to the board itself. But having the Pitch value twice the value, is confusing and - I assume - a bug. Can anyone confirm this or correct me if I am wrong?

If this is a bug, I suppose it is in SparkfunMPU9250-DMP.cpp #623-654 (MPU9250_DMP::computeEulerAngles)
By tedd
#193528
I'll answer this myself.

With the routine toEulerianAngle at https://en.wikipedia.org/wiki/Conversio ... ler_angles
I am getting consistent, correct Roll, Pitch and Yaw values. I still don't know if the imu.computeEulerAngles() routine is correct. Maybe it is, if I just could understand it.
As a bonus, the Roll and Pitch values from the toEulerianAngle are body angles, and not world angles. More practical.

Hope this helps anyone having the same issue.
By tedd
#193530
Thanks for confirming!

I suspect that the computeEulerAngles routine is buggy because for me it gives consistently erroneous values, while the new routine gives consistently correct values.
One solution is to copy the above mentioned routine into your own code and let it process the quaternions, bypassing the computeEulerAngles. Or do as I ended up doing, forwarding the quaternions to my PC and let it process it.
However, the best solution is of course to let the author fix the library so that it will benefit everyone using it. Or add the new routine to the library, if it turns out that the existing routine serves a purpose (correctly).
By szotyi
#193531
Thanks,

I have tried to implement, but without luck. Can you please help me out?
static void toEulerianAngle() {

double ysqr = imu.qy * imu.qy;

// roll (x-axis rotation)
double t0 = +2.0 * (imu.qw * imu.qx + imu.qy * imu.qz);
double t1 = +1.0 - 2.0 * (imu.qx * imu.qx + ysqr);

SerialPort.print("$RPY,");
SerialPort.print(atan2(t0, t1));

// pitch (y-axis rotation)
double t2 = +2.0 * (imu.qw * imu.qy - imu.qz * imu.qx);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
SerialPort.print(",");
SerialPort.print(asin(t2));

// yaw (z-axis rotation)
double t3 = +2.0 * (imu.qw * imu.qz + imu.qx * imu.qy);
double t4 = +1.0 - 2.0 * (ysqr + imu.qz * imu.qz);
SerialPort.print(",");
SerialPort.print(atan2(t3, t4));
SerialPort.println("*");
}

I have also tried to replace all imu.qValue with imu.calcQuat(imu.qValue), without luck. Thanks!
By tedd
#193532
Hi, this code is working for me.
As you mention, you need to work with the calcQuat() values, as they are the Q30 values from the sensor converted to floating point.
I can't see directly what went wrong in your code, but remember that the angles are in radians, not degrees.
Also, from what I can see, it is good to activate the accelerometer too so that the gyros relate to the plane perpendicular to gravity and are not free floating.
Code: Select all
/************************************************************
  MPU9250_DMP_Gyro_Cal
  Gyro calibration example for MPU-9250 DMP Arduino Library
  Jim Lindblom @ SparkFun Electronics
  original creation date: November 23, 2016
  https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library

  This example sketch demonstrates how to use the MPU-9250's
  digital motion processor (DMP) to calibrate the gyroscope.
  After eight seconds of no motion, the DMP will compute
  gyro biases and subtract them.

  Development environment specifics:
  Arduino IDE 1.6.12
  SparkFun 9DoF Razor IMU M0

  Supported Platforms:
  - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts)
*************************************************************/
#include <SparkFunMPU9250-DMP.h>
#define SerialPort SerialUSB
#define LEDPIN 13

MPU9250_DMP imu;

void setup()
{
  pinMode(LEDPIN, OUTPUT);
  SerialPort.begin(115200);

  // Call imu.begin() to verify communication and initialize
  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      SerialPort.println("Unable to communicate with MPU-9250");
      SerialPort.println("Check connections, and try again.");
      SerialPort.println();
      delay(5000);
    }
  }

  // DMP_FEATURE_TAP -- Tap detection
  // DMP_FEATURE_ANDROID_ORIENT -- Orientation (portrait/landscape) detection
  // DMP_FEATURE_LP_QUAT -- Accelerometer, low-power quaternion calculation
  // DMP_FEATURE_PEDOMETER -- Pedometer (always enabled)
  // DMP_FEATURE_6X_LP_QUAT -- 6-axis (accel/gyro) quaternion calculation
  // DMP_FEATURE_GYRO_CAL -- Gyroscope calibration (0's out after 8 seconds of no motion)
  // DMP_FEATURE_SEND_RAW_ACCEL -- Send raw accelerometer values to FIFO
  // DMP_FEATURE_SEND_RAW_GYRO -- Send raw gyroscope values to FIFO
  // DMP_FEATURE_SEND_CAL_GYRO -- Send calibrated gyroscop values to FIFO
  imu.dmpBegin(DMP_FEATURE_SEND_RAW_ACCEL |
               DMP_FEATURE_GYRO_CAL |        // Enable gyro calibration
               DMP_FEATURE_SEND_CAL_GYRO |   // Send calibrated gyro values
               DMP_FEATURE_6X_LP_QUAT,
               50);                   // Set DMP rate in Hz
}

static void toEulerianAngle(float w, float x, float y, float z, double& roll, double& pitch, double& yaw)
{
  double ysqr = y * y;

  // roll (x-axis rotation)
  double t0 = +2.0 * (w * x + y * z);
  double t1 = +1.0 - 2.0 * (x * x + ysqr);
  roll = atan2(t0, t1);

  // pitch (y-axis rotation)
  double t2 = +2.0 * (w * y - z * x);
  t2 = t2 > 1.0 ? 1.0 : t2;
  t2 = t2 < -1.0 ? -1.0 : t2;
  pitch = asin(t2);

  // yaw (z-axis rotation)
  double t3 = +2.0 * (w * z + x * y);
  double t4 = +1.0 - 2.0 * (ysqr + z * z);  
  yaw = atan2(t3, t4);
}


void loop()
{
  // Check for new data in the FIFO
  if ( imu.fifoAvailable() )
  {
    // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
    if ( imu.dmpUpdateFifo() == INV_SUCCESS)
    {
      String imuLog = ""; // Create a fresh line to log
      double roll;
      double pitch;
      double yaw;
      toEulerianAngle(imu.calcQuat(imu.qw), imu.calcQuat(imu.qx), imu.calcQuat(imu.qy), imu.calcQuat(imu.qz), roll, pitch, yaw);
      imuLog += String(roll, 6) + ",";
      imuLog += String(pitch, 6) + ",";
      imuLog += String(yaw, 6) + ",";
      SerialPort.println(imuLog);
    }
  }
}

By mdancer
#193538
If you look at SparkFunMPU9250-DMP.cpp line #641 it reads:

pitch = asin(t2) * 2;

The multiplication by 2 is an error and explains why your pitch is twice what it should be!
By mdancer
#193541
I'm not sure what you mean by "body angles". Euler angles define a specific order of rotations about principle axes that define the orientation of one reference coordinate system with respect to another - typically body frame relative to inertial frame. So for the case of roll, pitch, and yaw, to get from an inertial reference frame to the reference frame of the IMU, you'd rotate about the z axis by yaw, then about the (new) y axis by pitch, and finally about the (newer) x axis by roll. I work as an automatic flight controls engineer for an aircraft avionics supply company and I can't say I've ever heard the term "body angles" when referred to the Euler angles roll, pitch, and yaw.
By tedd
#193561
I am probably not using the correct technical terms. But I am comparing the original firmware Euler calculation with the Wikipedia one.
And apart from the * 2 bug for the Y axis, I also see another fundamental difference.
Let me explain first with the Wikipedia variant:
I orient the sensor physically so that Yaw=0, Pitch=0 and Roll=0 (flat on the desk). Then I Pitch it upwards 45 degrees (nose of board upwards). It results in Yaw=0, Pitch=45, Roll=0. Correct and expected.
Then I rotate the board 90 degrees around the Z-axis(gravity axis). Yaw is then 90, Pitch=45 and Roll=0. This is correct and expected.
However, notice the difference with the original Euler calculation:
I orient the sensor as earlier flat, then pitch it upwards 45 degrees. I read Yaw=0, Pitch=45, Roll=0. Same as earlier.
Then I rotate the board 90 degrees around the Z-axis(gravity axis). Yaw is then 90, Pitch=0 and Roll=45. This is different and surprising. Pitch seems to be relative to where the sensor believes 0 degrees is. The pitch angle shifted from pitch to roll.
I can only explain it that the Wikipedia code calculates Pitch and Roll relative to the body axis, while the other code relative to axis where X aligns with Yaw=0 ("world axis").
I am not saying that one of these are wrong, only that they are different in their axis definitions.
Both are usable, but you have to know about it. And it matters to get the rotation order correct for display.
However, I favor the Wikipedia one because I specifically need Roll and Pitch in local body coordinates so that I can present it to the user.
By mdancer
#193581
You may not be saying that one of them is wrong, but I will. If you place the board in the same attitude (orientation) and two separate algorithms give you different values for roll, pitch, and yaw, then one of those algorithms must be wrong!

You stated that for your experiment you started with the board flat on your desk. That would make it look like the left image below - both inertial reference frame and body reference frame are aligned. Then you pitch the board up 45 degrees to get to the right image below. No problems up to this point, right?
Figure1.png
Now things can go two different paths. You state that you next rotate about the z axis, but there are two z axes - one for the inertial frame and one for the body frame. You clarified the z axis as the gravity axis, so I'm assuming that your second rotation is about the inertial z axis (black). Thus the final attitude would be the left image below where yaw = 90, pitch = 45, and roll = 0. But if you actually rotated about the body z axis (blue) then you'd get the attitude in the right image below where yaw = 90, pitch = 0, and roll = 45.
Figure2.png
So two different values for roll, pitch, and yaw translate into two very different attitudes. If the final attitude of the board in your experiment is that of the left image above, then the Wiki implementation is correct. However, if the final attitude matches that of the right image, then the original implementation is correct.
You do not have the required permissions to view the files attached to this post.
By tedd
#193811
Thanks for the thorough description. It seems to me that the left image is the correct one. However, I'll make another attempt at explaining the difference between the two:
The wiki implementation behaves like the left image like this:
1. Sensor flat on the desk, Roll, Pitch, Yaw = 0
2. Pitch the sensor up 45 degrees, results in Roll=0, Pitch=45, Yaw=0.
3. I rotate physically around the gravity axis (inertial Z-axis, black) in a full turn.
This results in a changing Yaw value 0-359.9. Roll and Pitch stay with 0 and 45 degrees.

However, with the original firmware:
1. and 2. the same as above
3. I rotate physically around the gravity axis in a full turn. What I observe from the numbers is this:
Yaw is always correct. It changes 0-359.9 degrees as I make a complete rotation.
But:
When Yaw=0, Roll=0, Pitch=45
When Yaw=45, Roll=33, Pitch=33 (ca. numbers)
When Yaw=90, Roll=45, Pitch=0
When Yaw=180, Roll=0, Pitch=-45
When Yaw=270, Roll=-45, Pitch=0

In other words, I see unexpected roll and pitch values.