Use standard orientation for IMU axes

Author: okalachev

ICM20948.cpp

@@ -278,12 +278,6 @@ void ICM20948::getAccel(float& x, float& y, float& z) const {
 	x = x * G;
 	y = y * G;
 	z = z * G;
-	// orient axes to match MPU9250 library
-	// TODO: switch to internal accel and gyro axes
-	double tmp = x;
-	x = y;
-	y = tmp;
-	z = -z;
 }
 
 float ICM20948::getTemp() const {
@@ -304,12 +298,6 @@ void ICM20948::getGyro(float& x, float& y, float& z) const {
 	x = x * DEG_TO_RAD;
 	y = y * DEG_TO_RAD;
 	z = z * DEG_TO_RAD;
-	// orient axes to match MPU9250 library
-	// TODO: switch to internal accel and gyro axes
-	double tmp = x;
-	x = y;
-	y = tmp;
-	z = -z;
 }
 
 void ICM20948::getMag(float& x, float& y, float& z) const {
@@ -320,12 +308,10 @@ void ICM20948::getMag(float& x, float& y, float& z) const {
 	x = x * AK09916_MAG_LSB;
 	y = y * AK09916_MAG_LSB;
 	z = z * AK09916_MAG_LSB;
-	// orient axes to match MPU9250 library
-	// TODO: switch to internal accel and gyro axes
-	double tmp = x;
-	x = -y;
-	y = tmp;
-	z = z;
+	// orient magnetometer to match accel and gyro
+	x = x;
+	y = -y;
+	z = -z;
 }
 
 

MPU9250.cpp

@@ -453,20 +453,20 @@ bool MPU9250::read() {
       new_mag_data_ = false;
     }
   }
-  /* Convert to float values and rotate the accel / gyro axis */
-  accel_[0] = static_cast<float>(accel_cnts_[1]) * accel_scale_ * G_MPS2_;
-  accel_[1] = static_cast<float>(accel_cnts_[0]) * accel_scale_ * G_MPS2_;
-  accel_[2] = static_cast<float>(accel_cnts_[2]) * accel_scale_ * -1.0f *
-              G_MPS2_;
+  /* Convert to float values */
+  accel_[0] = static_cast<float>(accel_cnts_[0]) * accel_scale_ * G_MPS2_;
+  accel_[1] = static_cast<float>(accel_cnts_[1]) * accel_scale_ * G_MPS2_;
+  accel_[2] = static_cast<float>(accel_cnts_[2]) * accel_scale_ * G_MPS2_;
   temp_ = (static_cast<float>(temp_cnts_) - 21.0f) / TEMP_SCALE_ + 21.0f;
-  gyro_[0] = static_cast<float>(gyro_cnts_[1]) * gyro_scale_ * DEG2RAD_;
-  gyro_[1] = static_cast<float>(gyro_cnts_[0]) * gyro_scale_ * DEG2RAD_;
-  gyro_[2] = static_cast<float>(gyro_cnts_[2]) * gyro_scale_ * -1.0f * DEG2RAD_;
+  gyro_[0] = static_cast<float>(gyro_cnts_[0]) * gyro_scale_ * DEG2RAD_;
+  gyro_[1] = static_cast<float>(gyro_cnts_[1]) * gyro_scale_ * DEG2RAD_;
+  gyro_[2] = static_cast<float>(gyro_cnts_[2]) * gyro_scale_ * DEG2RAD_;
   /* Only update on new data */
   if (new_mag_data_) {
-    mag_[0] =   static_cast<float>(mag_cnts_[0]) * mag_scale_[0];
-    mag_[1] =   static_cast<float>(mag_cnts_[1]) * mag_scale_[1];
-    mag_[2] =   static_cast<float>(mag_cnts_[2]) * mag_scale_[2];
+    /* Orient magnetometer axes to match accel and gyro */
+    mag_[0] =   static_cast<float>(mag_cnts_[1]) * mag_scale_[0];
+    mag_[1] =   static_cast<float>(mag_cnts_[0]) * mag_scale_[1];
+    mag_[2] =   static_cast<float>(mag_cnts_[2]) * mag_scale_[2] * -1.0f;
   }
   return true;
 }

README.md

@@ -75,6 +75,14 @@ The ICM-20948 IMU driver has the same interface. Only the declaration is changed
 ICM20948 IMU(SPI);
 ```
 
+### IMU axes orientation
+
+Orientation of the IMU axes (including magnetometer) on various boards:
+
+|GY-91|MPU-9265|ICM-20948|
+|-|-|-|
+|<img src="img/gy91-axes.svg" width="200" alt="GY-91 axes orientation">|<img src="img/mpu9265-axes.svg" width="200" alt="MPU-9265 axes orientation">|<img src="img/icm20948-axes.svg" width="200" alt="ICM-20948 axes orientation">|
+
 ### Connection
 
 Connecting GY-91 board to ESP32 using VSPI: