From ca9d1a55fe5aac71e08092aa9ee4ead549e4e22e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Apr 2018 21:08:52 +1000 Subject: [PATCH] AP_InertialSensor: support ICM-20689 IMU like the 20789, but without the baro --- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 3 ++- .../AP_InertialSensor_Invensense.cpp | 13 ++++++++++++- .../AP_InertialSensor_Invensense.h | 1 + .../AP_InertialSensor_Invensense_registers.h | 1 + 4 files changed, 16 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 359bc35baf..912df3b781 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -102,7 +102,8 @@ public: DEVTYPE_GYR_MPU9250 = 0x24, DEVTYPE_GYR_I3G4250D = 0x25, DEVTYPE_GYR_LSM9DS1 = 0x26, - DEVTYPE_INS_ICM20789 = 0x27 + DEVTYPE_INS_ICM20789 = 0x27, + DEVTYPE_INS_ICM20689 = 0x28, }; protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 5d816c4d6c..c86580c937 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -215,6 +215,10 @@ void AP_InertialSensor_Invensense::start() gdev = DEVTYPE_INS_ICM20789; adev = DEVTYPE_INS_ICM20789; break; + case Invensense_ICM20689: + gdev = DEVTYPE_INS_ICM20689; + adev = DEVTYPE_INS_ICM20689; + break; } /* @@ -243,6 +247,10 @@ void AP_InertialSensor_Invensense::start() temp_zero = 25; temp_sensitivity = 0.003; break; + case Invensense_ICM20689: + temp_zero = 25; + temp_sensitivity = 0.003; + break; } _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev)); @@ -693,7 +701,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) // setup for 4kHz accels _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B, true); } else { - uint8_t fifo_size = (_mpu_type == Invensense_ICM20789) ? 1:0; + uint8_t fifo_size = (_mpu_type == Invensense_ICM20789 || _mpu_type == Invensense_ICM20689) ? 1:0; _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_DLPF_CFG_218HZ | (fifo_size<<6), true); } } @@ -726,6 +734,9 @@ bool AP_InertialSensor_Invensense::_check_whoami(void) case MPU_WHOAMI_ICM20789_R1: _mpu_type = Invensense_ICM20789; return true; + case MPU_WHOAMI_ICM20689: + _mpu_type = Invensense_ICM20689; + return true; } // not a value WHOAMI result return false; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index e6d541eea6..4d80129821 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -63,6 +63,7 @@ public: Invensense_ICM20608, Invensense_ICM20602, Invensense_ICM20789, + Invensense_ICM20689, }; private: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h index 421fc1b79c..1ef6b11891 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h @@ -174,6 +174,7 @@ #define MPU_WHOAMI_MPU9255 0x73 #define MPU_WHOAMI_ICM20789 0x03 #define MPU_WHOAMI_ICM20789_R1 0x02 +#define MPU_WHOAMI_ICM20689 0x98 #define BIT_READ_FLAG 0x80 #define BIT_I2C_SLVX_EN 0x80