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

AP_InertialSensor: added support for IIM-42652 and ICM-40605 sensors 

handle new Invensense v3 sensor types
This commit is contained in:
QioTek 2021-12-13 12:43:51 +08:00 committed by Andrew Tridgell
parent 7b90326888
commit 586c2064a6
3 changed files with 42 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -121,6 +121,8 @@ public:
DEVTYPE_INS_ICM40609 = 0x33,
DEVTYPE_INS_ICM42688 = 0x34,
DEVTYPE_INS_ICM42605 = 0x35,
DEVTYPE_INS_ICM40605 = 0x36,
DEVTYPE_INS_IIM42652 = 0x37,
};
protected:

38
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -19,6 +19,8 @@
ICM-40609
ICM-42688
ICM-42605
ICM-40605
IIM-42652
Note that this sensor includes 32kHz internal sampling and an
anti-aliasing filter, which means this driver can be a lot simpler
@ -62,9 +64,11 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
#define INV3REG_BANK_SEL 0x76
// WHOAMI values
#define INV3_ID_ICM40605 0x33
#define INV3_ID_ICM40609 0x3b
#define INV3_ID_ICM42605 0x42
#define INV3_ID_ICM42688 0x47
#define INV3_ID_IIM42652 0x6f
// run output data at 2kHz
#define INV3_ODR 2000
@ -127,7 +131,7 @@ void AP_InertialSensor_Invensensev3::fifo_reset()
// FIFO_MODE stop-on-full
register_write(INV3REG_FIFO_CONFIG, 0x80);
// FIFO partial disable, enable accel, gyro, temperature
register_write(INV3REG_FIFO_CONFIG1, 0x07);
register_write(INV3REG_FIFO_CONFIG1, fifo_config1);
// little-endian, fifo count in records, last data hold for ODR mismatch
register_write(INV3REG_INTF_CONFIG0, 0xC0);
register_write(INV3REG_SIGNAL_PATH_RESET, 2);
@ -143,24 +147,40 @@ void AP_InertialSensor_Invensensev3::start()
// initially run the bus at low speed
dev->set_speed(AP_HAL::Device::SPEED_LOW);
// always use FIFO
fifo_reset();
// grab the used instances
enum DevTypes devtype;
switch (inv3_type) {
case Invensensev3_Type::IIM42652:
devtype = DEVTYPE_INS_IIM42652;
fifo_config1 = 0x07;
temp_sensitivity = 1.0 / 2.07;
break;
case Invensensev3_Type::ICM42688:
devtype = DEVTYPE_INS_ICM42688;
fifo_config1 = 0x07;
temp_sensitivity = 1.0 / 2.07;
break;
case Invensensev3_Type::ICM42605:
devtype = DEVTYPE_INS_ICM42605;
fifo_config1 = 0x07;
temp_sensitivity = 1.0 / 2.07;
break;
case Invensensev3_Type::ICM40605:
devtype = DEVTYPE_INS_ICM40605;
fifo_config1 = 0x0F;
temp_sensitivity = 1.0 * 128 / 115.49;
break;
case Invensensev3_Type::ICM40609:
default:
devtype = DEVTYPE_INS_ICM40609;
temp_sensitivity = 1.0 / 2.07;
fifo_config1 = 0x07;
break;
}
// always use FIFO
fifo_reset();
if (!_imu.register_gyro(gyro_instance, INV3_ODR, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, INV3_ODR, dev->get_bus_id_devtype(devtype))) {
return;
@ -343,6 +363,14 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void)
inv3_type = Invensensev3_Type::ICM42605;
accel_scale = (GRAVITY_MSS / 2048);
return true;
case INV3_ID_ICM40605:
inv3_type = Invensensev3_Type::ICM40605;
accel_scale = (GRAVITY_MSS / 2048);
return true;
case INV3_ID_IIM42652:
inv3_type = Invensensev3_Type::IIM42652;
accel_scale = (GRAVITY_MSS / 2048);
return true;
}
// not a value WHOAMI result
return false;
@ -368,7 +396,9 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
_clip_limit = 29.5f * GRAVITY_MSS;
break;
case Invensensev3_Type::ICM42688:
case Invensensev3_Type::IIM42652:
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::ICM40605:
_clip_limit = 15.5f * GRAVITY_MSS;
break;
}

7
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -33,6 +33,8 @@ public:
ICM40609 = 0,
ICM42688,
ICM42605,
ICM40605,
IIM42652,
};
// acclerometers on Invensense sensors will return values up to 32G
@ -63,8 +65,11 @@ private:
uint8_t gyro_instance;
uint8_t accel_instance;
// reset FIFO configure1 register
uint8_t fifo_config1;
// temp scaling for FIFO temperature
const float temp_sensitivity = 1.0f/2.07;
float temp_sensitivity;
const float temp_zero = 25; // degC
const enum Rotation rotation;