AP_InertialSensor: Rework rotation handling

Right now, MPU based imus perform the rotations in the driver in an
implicit way when converting sensor bus data to actual x, y, z data.
MPU6000 and MPU9250 :
x = y, y = x, z = -z
meaning ROTATION_ROLL_180_YAW_90
LSM9DS0 and L3G4200 :
x = x, y = -y, z = -z
meaning ROTATION ROLL_180

This commit removes these implict rotations from the drivers and modify the
per-board orientations so the behaviour doesn't change, by applying a
combination of ROLL_180_YAW_90 and then the former board rotation for MPU9250
and MPU6000 and applying a combination of ROLL_180 and the former board rotation
for LSM9DS0 and L3G4200.

RASPILOT had a compilation flag inside the LSM9DS0
driver to add a YAW_90 rotation. When added to the ROLL_180 applied when
converting data, it gives the same rotation as the computed rotation for the
mpu6000, i.e ROLL_180_YAW_90

libraries/AP_HAL/board/linux.h

@@ -18,9 +18,9 @@
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
-#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_270
+#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_180
 #else
-#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
+#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
 #endif
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
@@ -37,7 +37,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/data/ftp/internal_000/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/data/ftp/internal_000/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_MPU60XX_I2C
-#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_270
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180
 #define HAL_INS_MPU60x0_I2C_BUS 2
 #define HAL_INS_MPU60x0_I2C_ADDR 0x68
 #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C
@@ -79,7 +79,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/data/ftp/internal_000/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/data/ftp/internal_000/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_MPU60XX_I2C
-#define HAL_INS_DEFAULT_ROTATION ROTATION_PITCH_180_YAW_90
+#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_180
 #define HAL_INS_MPU60x0_I2C_BUS 2
 #define HAL_INS_MPU60x0_I2C_ADDR 0x68
 #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C
@@ -122,7 +122,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_MPU60XX_SPI
-#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_90
+#define HAL_INS_DEFAULT_ROTATION ROTATION_PITCH_180
 #define HAL_INS_MPU60x0_NAME "mpu6000"
 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
 #define HAL_BARO_MS5611_I2C_BUS 10
@@ -148,7 +148,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
-#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
 #define HAL_BARO_MS5611_I2C_BUS 1
@@ -161,7 +161,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
-#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
 #define HAL_BARO_MS5611_I2C_BUS 1
@@ -178,6 +178,7 @@
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
 #define HAL_INS_DEFAULT HAL_INS_RASPILOT
 #define HAL_INS_MPU60x0_NAME "mpu6000"
 #define HAL_INS_LSM9DS0_G_NAME "lsm9ds0_g"
@@ -216,7 +217,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT HAL_INS_BBBMINI
-#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_INS_MPU9250_NAME_EXT "mpu9250ext"
 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
@@ -248,6 +249,7 @@
 #define HAL_BARO_MS5611_I2C_BUS 1
 #define HAL_BARO_MS5611_I2C_ADDR 0x77
 #define HAL_INS_DEFAULT HAL_INS_BH
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_INS_MPU60x0_I2C_BUS 1
 #define HAL_INS_MPU60x0_I2C_ADDR 0x69
@@ -274,7 +276,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT           HAL_INS_MPU9250_SPI
-#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_270
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_BARO_DEFAULT          HAL_BARO_MS5611_SPI
 #define HAL_BARO_MS5611_NAME "ms5611"
@@ -301,7 +303,7 @@
 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
 #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
 #define HAL_INS_DEFAULT           HAL_INS_MPU9250_SPI
-#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
+#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
 #define HAL_INS_MPU9250_NAME "mpu9250"
 #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -686,24 +686,24 @@ AP_InertialSensor::detect_backends(void)
 #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
 
     if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1) {
-        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
+        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180_YAW_90));
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) {
-        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
+        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));
         _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
                                                       hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
-                                                      hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_ROLL_180));
+                                                      hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
         // older Pixhawk2 boards have the MPU6000 instead of MPU9250
-        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180))) {
-            _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_PITCH_180));
+        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_YAW_90))) {
+            _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_YAW_90));
         }
         _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
                                                       hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
-                                                      hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_ROLL_180_YAW_270));
-        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) {
-            _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));
+                                                      hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270));
+        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180))) {
+            _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
         }
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
@@ -712,29 +712,37 @@ AP_InertialSensor::detect_backends(void)
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) {
         // PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
-        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
-        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
+        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_YAW_270));
+        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) {
-        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
+        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
     }
     // also add any PX4 backends (eg. canbus sensors)
     _add_backend(AP_InertialSensor_PX4::detect(*this));
 #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION)
     _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
 #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
     _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
+#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0 && defined(HAL_INS_DEFAULT_ROTATION)
+    _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
+                 hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
+                 hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), HAL_INS_DEFAULT_ROTATION));
 #elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
     _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
                  hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
                  hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
+#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D && defined(HAL_INS_DEFAULT_ROTATION)
+    _add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR), HAL_INS_DEFAULT_ROTATION));
 #elif HAL_INS_DEFAULT == HAL_INS_L3G4200D
     _add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)));
 #elif HAL_INS_DEFAULT == HAL_INS_RASPILOT
-    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
+    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)), HAL_INS_DEFAULT_ROTATION);
     _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
                  hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
-                 hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
+                 hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), HAL_INS_DEFAULT_ROTATION));
+#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C && defined(HAL_INS_DEFAULT_ROTATION)
+    _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR), HAL_INS_DEFAULT_ROTATION));
 #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
     _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
 #elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -251,7 +251,7 @@ bool AP_InertialSensor_L3G4200D::_accumulate(void)
         if (!_dev->read_registers(L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT,
                                   (uint8_t *)&buffer, sizeof(buffer))) {
             for (uint8_t i=0; i < num_samples_available; i++) {
-                Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
+                Vector3f gyro = Vector3f(buffer[i][0], buffer[i][1], buffer[i][2]);
                 // Adjust for chip scaling to get radians/sec
                 gyro *= L3G4200D_GYRO_SCALE_R_S;
                 _rotate_and_correct_gyro(_gyro_instance, gyro);

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -755,13 +755,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
         return;
     }
 
-    Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
-
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-    // LSM303D on RasPilot
-    // FIXME: wrong way to provide rotation per-board
-    gyro_data.rotate(ROTATION_YAW_90);
-#endif
+    Vector3f gyro_data(raw_data.x, raw_data.y, raw_data.z);
 
     gyro_data *= _gyro_scale;
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -487,18 +487,18 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
         fsync_set = (int16_val(data, 2) & 1U) != 0;
 #endif
 
-        accel = Vector3f(int16_val(data, 1),
-                         int16_val(data, 0),
-                         -int16_val(data, 2));
+        accel = Vector3f(int16_val(data, 0),
+                         int16_val(data, 1),
+                         int16_val(data, 2));
         accel *= _accel_scale;
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
         _last_temp = temp;
 
-        gyro = Vector3f(int16_val(data, 5),
-                        int16_val(data, 4),
-                        -int16_val(data, 6));
+        gyro = Vector3f(int16_val(data, 4),
+                        int16_val(data, 5),
+                        int16_val(data, 6));
         gyro *= GYRO_SCALE;
 
         _rotate_and_correct_accel(_accel_instance, accel);
@@ -519,17 +519,17 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
 
     for (uint8_t i = 0; i < n_samples; i++) {
         uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
-        Vector3l a(int16_val(data, 1),
-                   int16_val(data, 0),
-                   -int16_val(data, 2));
+        Vector3l a(int16_val(data, 0),
+                   int16_val(data, 1),
+                   int16_val(data, 2));
         if (abs(a.x) > clip_limit ||
             abs(a.y) > clip_limit ||
             abs(a.z) > clip_limit) {
             clipped = true;
         }
-        Vector3l g(int16_val(data, 5),
-                   int16_val(data, 4),
-                   -int16_val(data, 6));
+        Vector3l g(int16_val(data, 4),
+                   int16_val(data, 5),
+                   int16_val(data, 6));
 
         _accum.accel += a;
         _accum.gyro += g;

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -417,18 +417,18 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
         uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
         Vector3f accel, gyro;
 
-        accel = Vector3f(int16_val(data, 1),
-                         int16_val(data, 0),
-                         -int16_val(data, 2));
+        accel = Vector3f(int16_val(data, 0),
+                         int16_val(data, 1),
+                         int16_val(data, 2));
         accel *= MPU9250_ACCEL_SCALE_1G;
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
         _last_temp = temp;
 
-        gyro = Vector3f(int16_val(data, 5),
-                        int16_val(data, 4),
-                        -int16_val(data, 6));
+        gyro = Vector3f(int16_val(data, 4),
+                        int16_val(data, 5),
+                        int16_val(data, 6));
         gyro *= GYRO_SCALE;
 
         _rotate_and_correct_accel(_accel_instance, accel);
@@ -450,17 +450,17 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
 
     for (uint8_t i = 0; i < n_samples; i++) {
         uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
-        Vector3l a(int16_val(data, 1),
-                   int16_val(data, 0),
-                   -int16_val(data, 2));
+        Vector3l a(int16_val(data, 0),
+                   int16_val(data, 1),
+                   int16_val(data, 2));
         if (abs(a.x) > clip_limit ||
             abs(a.y) > clip_limit ||
             abs(a.z) > clip_limit) {
             clipped = true;
         }
-        Vector3l g(int16_val(data, 5),
-                   int16_val(data, 4),
-                   -int16_val(data, 6));
+        Vector3l g(int16_val(data, 4),
+                   int16_val(data, 5),
+                   int16_val(data, 6));
 
         _accum.accel += a;
         _accum.gyro += g;