Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IneertialSensor: fixed orientations and gyro scale factors for new Invensense IMUs

This commit is contained in:
Andrew Tridgell 2019-03-14 16:47:45 +11:00
parent 8c2d3945de
commit 0a718ba330
3 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -743,8 +743,8 @@ AP_InertialSensor::detect_backends(void)
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
// new cubes have ICM20602, ICM20648, ICM20649
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602_ext"), ROTATION_ROLL_180_YAW_270));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948_ext"), ROTATION_ROLL_180_YAW_270));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948"), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948_ext"), ROTATION_PITCH_180));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948"), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_FMUV5:

18
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -172,24 +172,24 @@ void AP_InertialSensor_Invensensev2::start()
// grab the used instances
enum DevTypes gdev, adev;
switch (_inv2_type) {
case Invensensev2_ICM20948:
gdev = DEVTYPE_INS_ICM20948;
adev = DEVTYPE_INS_ICM20948;
_accel_scale = (GRAVITY_MSS / 2048.f);
break;
case Invensensev2_ICM20648:
gdev = DEVTYPE_INS_ICM20648;
adev = DEVTYPE_INS_ICM20648;
_accel_scale = (GRAVITY_MSS / 2048.f);
// using 16g full range, 2048 LSB/g
_accel_scale = (GRAVITY_MSS / 2048);
break;
case Invensensev2_ICM20649:
// 20649 is setup for 30g full scale, 1024 LSB/g
gdev = DEVTYPE_INS_ICM20649;
adev = DEVTYPE_INS_ICM20649;
_accel_scale = (GRAVITY_MSS / 1024.f);
_accel_scale = (GRAVITY_MSS / 1024);
break;
case Invensensev2_ICM20948:
default:
gdev = DEVTYPE_INS_ICM20948;
adev = DEVTYPE_INS_ICM20948;
// using 16g full range, 2048 LSB/g
_accel_scale = (GRAVITY_MSS / 2048);
break;
}
@ -563,8 +563,8 @@ void AP_InertialSensor_Invensensev2::_select_bank(uint8_t bank)
*/
void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
{
uint8_t gyro_config = BITS_GYRO_FS_2000DPS;
uint8_t accel_config = BITS_ACCEL_FS_16G;
uint8_t gyro_config = (_inv2_type == Invensensev2_ICM20649)?BITS_GYRO_FS_2000DPS_20649 : BITS_GYRO_FS_2000DPS;
uint8_t accel_config = (_inv2_type == Invensensev2_ICM20649)?BITS_ACCEL_FS_30G_20649:BITS_ACCEL_FS_16G;
// assume 1.125kHz sampling to start
_fifo_downsample_rate = 1;

2
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2_registers.h
View File

@ -138,6 +138,7 @@
# define BITS_GYRO_FS_500DPS 0x02
# define BITS_GYRO_FS_1000DPS 0x04
# define BITS_GYRO_FS_2000DPS 0x06
# define BITS_GYRO_FS_2000DPS_20649 0x04
# define BITS_GYRO_FS_MASK 0x06 // only bits 1 and 2 are used for gyro full scale so use this to mask off other bits
#define INV2REG_GYRO_CONFIG_2 INV2REG(REG_BANK2,0x02U)
#define INV2REG_XG_OFFS_USRH INV2REG(REG_BANK2,0x03U)
@ -166,6 +167,7 @@
# define BITS_ACCEL_FS_4G 0x02
# define BITS_ACCEL_FS_8G 0x04
# define BITS_ACCEL_FS_16G 0x06
# define BITS_ACCEL_FS_30G_20649 0x06
# define BITS_ACCEL_FS_MASK 0x06 // only bits 1 and 2 are used for gyro full scale so use this to mask off other bits
#define INV2REG_FSYNC_CONFIG INV2REG(REG_BANK2,0x52U)
# define FSYNC_CONFIG_EXT_SYNC_TEMP 0x01