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

AP_IntertialSensor: support ICM20601 IMU

Browse Source
This commit is contained in:
na 2019-03-25 13:25:33 -05:00 committed by Andrew Tridgell
parent 2d2b02e50b
commit 494a3f86ad
4 changed files with 34 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -107,6 +107,7 @@ public:
DEVTYPE_INS_ICM20648 = 0x2D,
DEVTYPE_INS_ICM20649 = 0x2E,
DEVTYPE_INS_ICM20602 = 0x2F,
DEVTYPE_INS_ICM20601 = 0x30,
};
protected:

48
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -59,12 +59,6 @@ extern const AP_HAL::HAL& hal;
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
/*
* RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
*/
static const float GYRO_SCALE = (0.0174532f / 16.4f);
/*
* RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
* accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
@ -205,6 +199,10 @@ void AP_InertialSensor_Invensense::start()
gdev = DEVTYPE_INS_ICM20602;
adev = DEVTYPE_INS_ICM20602;
break;
case Invensense_ICM20601:
gdev = DEVTYPE_INS_ICM20601;
adev = DEVTYPE_INS_ICM20601;
break;
case Invensense_MPU6000:
case Invensense_MPU6500:
case Invensense_ICM20608:
@ -240,6 +238,7 @@ void AP_InertialSensor_Invensense::start()
case Invensense_ICM20608:
case Invensense_ICM20602:
case Invensense_ICM20601:
temp_zero = 25;
temp_sensitivity = 1.0/326.8;
break;
@ -294,18 +293,27 @@ void AP_InertialSensor_Invensense::start()
// Rev C has different scaling than rev D
_register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
_accel_scale = GRAVITY_MSS / 4096.f;
_gyro_scale = (radians(1) / 16.4f);
} else if (_mpu_type == Invensense_ICM20601) {
// Accel scale 32g (4096 LSB/g)
_register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
_accel_scale = GRAVITY_MSS / 4096.f;
_gyro_scale = (radians(1) / 8.2f);
_clip_limit = 29.5f * GRAVITY_MSS;
} else {
// Accel scale 16g (2048 LSB/g)
_register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
_accel_scale = GRAVITY_MSS / 2048.f;
_gyro_scale = (radians(1) / 16.4f);
}
hal.scheduler->delay(1);
if (_mpu_type == Invensense_ICM20608 ||
_mpu_type == Invensense_ICM20602) {
if (_mpu_type == Invensense_ICM20608 ||
_mpu_type == Invensense_ICM20602 ||
_mpu_type == Invensense_ICM20601) {
// this avoids a sensor bug, see description above
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
// configure interrupt to fire when new data arrives
_register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
@ -332,7 +340,7 @@ void AP_InertialSensor_Invensense::start()
// setup scale factors for fifo data after downsampling
_fifo_accel_scale = _accel_scale / (MAX(_fifo_downsample_rate,2)/2);
_fifo_gyro_scale = GYRO_SCALE / _fifo_downsample_rate;
_fifo_gyro_scale = _gyro_scale / _fifo_downsample_rate;
// allocate fifo buffer
_fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
@ -429,7 +437,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
gyro *= GYRO_SCALE;
gyro *= _gyro_scale;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
@ -489,7 +497,7 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
int16_val(data, 4),
-int16_val(data, 6));
Vector3f g2 = g * GYRO_SCALE;
Vector3f g2 = g * _gyro_scale;
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_accum.gyro += _accum.gyro_filter.apply(g);
@ -709,7 +717,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
config |= MPUREG_CONFIG_FIFO_MODE_STOP;
_register_write(MPUREG_CONFIG, config, true);
if (_mpu_type != Invensense_MPU6000) {
if (_mpu_type != Invensense_MPU6000) {
if (_fast_sampling) {
// setup for 4kHz accels
_register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B, true);
@ -743,6 +751,9 @@ bool AP_InertialSensor_Invensense::_check_whoami(void)
case MPU_WHOAMI_20602:
_mpu_type = Invensense_ICM20602;
return true;
case MPU_WHOAMI_20601:
_mpu_type = Invensense_ICM20601;
return true;
case MPU_WHOAMI_ICM20789:
case MPU_WHOAMI_ICM20789_R1:
_mpu_type = Invensense_ICM20789;
@ -832,11 +843,12 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
return false;
}
if (_mpu_type == Invensense_ICM20608 ||
_mpu_type == Invensense_ICM20602) {
if (_mpu_type == Invensense_ICM20608 ||
_mpu_type == Invensense_ICM20602 ||
_mpu_type == Invensense_ICM20601) {
// this avoids a sensor bug, see description above
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
_dev->get_semaphore()->give();
return true;

2
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -62,6 +62,7 @@ public:
Invensense_MPU9250,
Invensense_ICM20608,
Invensense_ICM20602,
Invensense_ICM20601,
Invensense_ICM20789,
Invensense_ICM20689,
};
@ -116,6 +117,7 @@ private:
float _temp_filtered;
float _accel_scale;
float _gyro_scale;
float _fifo_accel_scale;
float _fifo_gyro_scale;

1
libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h
View File

@ -169,6 +169,7 @@
#define MPU_WHOAMI_6000 0x68
#define MPU_WHOAMI_20608 0xaf
#define MPU_WHOAMI_20602 0x12
#define MPU_WHOAMI_20601 0xac
#define MPU_WHOAMI_6500 0x70
#define MPU_WHOAMI_MPU9250 0x71
#define MPU_WHOAMI_MPU9255 0x73