aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: use AP_HAL rotations for MPU6000

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-09 12:36:47 +11:00
parent fd8955a737
commit 84c3e29891
2 changed files with 6 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -664,8 +664,14 @@ AP_InertialSensor::detect_backends(void)
_add_backend(AP_InertialSensor_SITL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
HAL_INS_DEFAULT_ROTATION);
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
HAL_INS_DEFAULT_ROTATION);
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_BH

28
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -499,20 +499,6 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
-int16_val(data, 5));
gyro *= GYRO_SCALE;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
accel.rotate(ROTATION_YAW_270);
gyro.rotate(ROTATION_YAW_270);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
accel.rotate(ROTATION_YAW_90);
gyro.rotate(ROTATION_YAW_90);
#endif
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
@ -541,20 +527,6 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
float gscale = GYRO_SCALE / n_samples;
Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale);
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
accel.rotate(ROTATION_YAW_270);
gyro.rotate(ROTATION_YAW_270);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
accel.rotate(ROTATION_YAW_90);
gyro.rotate(ROTATION_YAW_90);
#endif
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);