From 494a3f86adf042209836b3108eecabe94b7ac784 Mon Sep 17 00:00:00 2001 From: na Date: Mon, 25 Mar 2019 13:25:33 -0500 Subject: [PATCH] AP_IntertialSensor: support ICM20601 IMU --- .../AP_InertialSensor_Backend.h | 1 + .../AP_InertialSensor_Invensense.cpp | 48 ++++++++++++------- .../AP_InertialSensor_Invensense.h | 2 + .../AP_InertialSensor_Invensense_registers.h | 1 + 4 files changed, 34 insertions(+), 18 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 62affba904..e3ba9b6efb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -107,6 +107,7 @@ public: DEVTYPE_INS_ICM20648 = 0x2D, DEVTYPE_INS_ICM20649 = 0x2E, DEVTYPE_INS_ICM20602 = 0x2F, + DEVTYPE_INS_ICM20601 = 0x30, }; protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 88d66442b5..086d690813 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index bdc14a4c5e..0f114e054c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h index 1ef6b11891..a9eb779f97 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h @@ -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