From 9f3fae8afd829cbec44d34f99cbc5ba64a611932 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Thu, 15 Aug 2024 09:54:15 +0100 Subject: [PATCH 1/2] Correct Euler (Tait Bryan) angles - see #145 --- .../Example11_DMP_Bias_Save_Restore_ESP32.ino | 39 +++++++++++++++---- .../Example7_DMP_Quat6_EulerAngles.ino | 37 ++++++++++++++---- 2 files changed, 61 insertions(+), 15 deletions(-) diff --git a/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino b/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino index 1daa489..bd8912c 100644 --- a/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino +++ b/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino @@ -321,24 +321,49 @@ void loop() SERIAL_PORT.print(F(" Accuracy:")); SERIAL_PORT.println(data.Quat9.Data.Accuracy); */ - + + // The ICM 20948 chip has axes y-forward, x-right and Z-up - see Figure 12: + // Orientation of Axes of Sensitivity and Polarity of Rotation + // in DS-000189-ICM-20948-v1.6.pdf These are the axes for gyro and accel and quat + // + // For conversion to roll, pitch and yaw for the equations below, the coordinate frame + // must be in aircraft reference frame. + // + // We use the Tait Bryan angles (in terms of flight dynamics): + // ref: https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles + // + // Heading – ψ : rotation about the Z-axis (+/- 180 deg.) + // Pitch – θ : rotation about the new Y-axis (+/- 90 deg.) + // Bank – ϕ : rotation about the new X-axis (+/- 180 deg.) + // + // where the X-axis points forward (pin 1 on chip), Y-axis to the right and Z-axis downward. + // In the conversion example above the rotation occurs in the order heading, pitch, bank. + // To get the roll, pitch and yaw equations to work properly we need to exchange the axes + + // Note when pitch approaches +/- 90 deg. the heading and bank become less meaningfull because the + // device is pointing up/down. (Gimbal lock) + double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - double q2sqr = q2 * q2; + + double qw = q0; // See issue #145 - thank you @Gord1 + double qx = q2; + double qy = q1; + double qz = -q3; // roll (x-axis rotation) - double t0 = +2.0 * (q0 * q1 + q2 * q3); - double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr); + double t0 = +2.0 * (qw * qx + qy * qz); + double t1 = +1.0 - 2.0 * (qx * qx + qy * qy); double roll = atan2(t0, t1) * 180.0 / PI; // pitch (y-axis rotation) - double t2 = +2.0 * (q0 * q2 - q3 * q1); + double t2 = +2.0 * (qw * qy - qx * qz); t2 = t2 > 1.0 ? 1.0 : t2; t2 = t2 < -1.0 ? -1.0 : t2; double pitch = asin(t2) * 180.0 / PI; // yaw (z-axis rotation) - double t3 = +2.0 * (q0 * q3 + q1 * q2); - double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3); + double t3 = +2.0 * (qw * qz + qx * qy); + double t4 = +1.0 - 2.0 * (qy * qy + qz * qz); double yaw = atan2(t3, t4) * 180.0 / PI; SERIAL_PORT.print(F("Roll: ")); diff --git a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino index 7cfccf1..ec973f0 100644 --- a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino +++ b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino @@ -222,27 +222,48 @@ void loop() SERIAL_PORT.println(q3, 3); */ - // Convert the quaternions to Euler angles (roll, pitch, yaw) - // https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles§ion=8#Source_code_2 + // The ICM 20948 chip has axes y-forward, x-right and Z-up - see Figure 12: + // Orientation of Axes of Sensitivity and Polarity of Rotation + // in DS-000189-ICM-20948-v1.6.pdf These are the axes for gyro and accel and quat + // + // For conversion to roll, pitch and yaw for the equations below, the coordinate frame + // must be in aircraft reference frame. + // + // We use the Tait Bryan angles (in terms of flight dynamics): + // ref: https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles + // + // Heading – ψ : rotation about the Z-axis (+/- 180 deg.) + // Pitch – θ : rotation about the new Y-axis (+/- 90 deg.) + // Bank – ϕ : rotation about the new X-axis (+/- 180 deg.) + // + // where the X-axis points forward (pin 1 on chip), Y-axis to the right and Z-axis downward. + // In the conversion example above the rotation occurs in the order heading, pitch, bank. + // To get the roll, pitch and yaw equations to work properly we need to exchange the axes + + // Note when pitch approaches +/- 90 deg. the heading and bank become less meaningfull because the + // device is pointing up/down. (Gimbal lock) double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - double q2sqr = q2 * q2; + double qw = q0; // See issue #145 - thank you @Gord1 + double qx = q2; + double qy = q1; + double qz = -q3; // roll (x-axis rotation) - double t0 = +2.0 * (q0 * q1 + q2 * q3); - double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr); + double t0 = +2.0 * (qw * qx + qy * qz); + double t1 = +1.0 - 2.0 * (qx * qx + qy * qy); double roll = atan2(t0, t1) * 180.0 / PI; // pitch (y-axis rotation) - double t2 = +2.0 * (q0 * q2 - q3 * q1); + double t2 = +2.0 * (qw * qy - qx * qz); t2 = t2 > 1.0 ? 1.0 : t2; t2 = t2 < -1.0 ? -1.0 : t2; double pitch = asin(t2) * 180.0 / PI; // yaw (z-axis rotation) - double t3 = +2.0 * (q0 * q3 + q1 * q2); - double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3); + double t3 = +2.0 * (qw * qz + qx * qy); + double t4 = +1.0 - 2.0 * (qy * qy + qz * qz); double yaw = atan2(t3, t4) * 180.0 / PI; #ifndef QUAT_ANIMATION From b0199f92e9fa198cae15648b3597a31d8ee52f2c Mon Sep 17 00:00:00 2001 From: PaulZC Date: Thu, 15 Aug 2024 09:54:43 +0100 Subject: [PATCH 2/2] v1.2.13 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index e832dd0..e659375 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library -version=1.2.12 +version=1.2.13 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™).