mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_IneertialSensor: fixed orientations and gyro scale factors for new Invensense IMUs
This commit is contained in:
parent
8c2d3945de
commit
0a718ba330
@ -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));
|
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
|
||||||
// new cubes have ICM20602, ICM20648, ICM20649
|
// 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_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_ext"), ROTATION_PITCH_180));
|
||||||
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"), ROTATION_YAW_270));
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case AP_BoardConfig::PX4_BOARD_FMUV5:
|
case AP_BoardConfig::PX4_BOARD_FMUV5:
|
||||||
|
@ -172,24 +172,24 @@ void AP_InertialSensor_Invensensev2::start()
|
|||||||
// grab the used instances
|
// grab the used instances
|
||||||
enum DevTypes gdev, adev;
|
enum DevTypes gdev, adev;
|
||||||
switch (_inv2_type) {
|
switch (_inv2_type) {
|
||||||
case Invensensev2_ICM20948:
|
|
||||||
gdev = DEVTYPE_INS_ICM20948;
|
|
||||||
adev = DEVTYPE_INS_ICM20948;
|
|
||||||
_accel_scale = (GRAVITY_MSS / 2048.f);
|
|
||||||
break;
|
|
||||||
case Invensensev2_ICM20648:
|
case Invensensev2_ICM20648:
|
||||||
gdev = DEVTYPE_INS_ICM20648;
|
gdev = DEVTYPE_INS_ICM20648;
|
||||||
adev = 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;
|
break;
|
||||||
case Invensensev2_ICM20649:
|
case Invensensev2_ICM20649:
|
||||||
|
// 20649 is setup for 30g full scale, 1024 LSB/g
|
||||||
gdev = DEVTYPE_INS_ICM20649;
|
gdev = DEVTYPE_INS_ICM20649;
|
||||||
adev = DEVTYPE_INS_ICM20649;
|
adev = DEVTYPE_INS_ICM20649;
|
||||||
_accel_scale = (GRAVITY_MSS / 1024.f);
|
_accel_scale = (GRAVITY_MSS / 1024);
|
||||||
break;
|
break;
|
||||||
|
case Invensensev2_ICM20948:
|
||||||
default:
|
default:
|
||||||
gdev = DEVTYPE_INS_ICM20948;
|
gdev = DEVTYPE_INS_ICM20948;
|
||||||
adev = DEVTYPE_INS_ICM20948;
|
adev = DEVTYPE_INS_ICM20948;
|
||||||
|
// using 16g full range, 2048 LSB/g
|
||||||
|
_accel_scale = (GRAVITY_MSS / 2048);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -563,8 +563,8 @@ void AP_InertialSensor_Invensensev2::_select_bank(uint8_t bank)
|
|||||||
*/
|
*/
|
||||||
void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
|
void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
|
||||||
{
|
{
|
||||||
uint8_t gyro_config = BITS_GYRO_FS_2000DPS;
|
uint8_t gyro_config = (_inv2_type == Invensensev2_ICM20649)?BITS_GYRO_FS_2000DPS_20649 : BITS_GYRO_FS_2000DPS;
|
||||||
uint8_t accel_config = BITS_ACCEL_FS_16G;
|
uint8_t accel_config = (_inv2_type == Invensensev2_ICM20649)?BITS_ACCEL_FS_30G_20649:BITS_ACCEL_FS_16G;
|
||||||
|
|
||||||
// assume 1.125kHz sampling to start
|
// assume 1.125kHz sampling to start
|
||||||
_fifo_downsample_rate = 1;
|
_fifo_downsample_rate = 1;
|
||||||
|
@ -138,6 +138,7 @@
|
|||||||
# define BITS_GYRO_FS_500DPS 0x02
|
# define BITS_GYRO_FS_500DPS 0x02
|
||||||
# define BITS_GYRO_FS_1000DPS 0x04
|
# define BITS_GYRO_FS_1000DPS 0x04
|
||||||
# define BITS_GYRO_FS_2000DPS 0x06
|
# 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 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_GYRO_CONFIG_2 INV2REG(REG_BANK2,0x02U)
|
||||||
#define INV2REG_XG_OFFS_USRH INV2REG(REG_BANK2,0x03U)
|
#define INV2REG_XG_OFFS_USRH INV2REG(REG_BANK2,0x03U)
|
||||||
@ -166,6 +167,7 @@
|
|||||||
# define BITS_ACCEL_FS_4G 0x02
|
# define BITS_ACCEL_FS_4G 0x02
|
||||||
# define BITS_ACCEL_FS_8G 0x04
|
# define BITS_ACCEL_FS_8G 0x04
|
||||||
# define BITS_ACCEL_FS_16G 0x06
|
# 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 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 INV2REG_FSYNC_CONFIG INV2REG(REG_BANK2,0x52U)
|
||||||
# define FSYNC_CONFIG_EXT_SYNC_TEMP 0x01
|
# define FSYNC_CONFIG_EXT_SYNC_TEMP 0x01
|
||||||
|
Loading…
Reference in New Issue
Block a user