fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 11:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4 icm42688p:Add Support for the ICM42686

Browse Source 
icm42688p:Fix colission on custom1 from rebase
This commit is contained in:
David Sidrane
2022-10-05 06:27:17 -07:00
committed by Daniel Agar
parent 9c467198be
commit 409c1bbf7a
5 changed files with 57 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_sensor.h
+2
View File
@@ -81,6 +81,8 @@
#define DRV_IMU_DEVTYPE_ICM42670P 0x2A
#define DRV_IMU_DEVTYPE_IIM42652 0x2B
#define DRV_IMU_DEVTYPE_IAM20680HP 0x2C
#define DRV_IMU_DEVTYPE_ICM42686P 0x2D
#define DRV_RNG_DEVTYPE_MB12XX 0x31
#define DRV_RNG_DEVTYPE_LL40LS 0x32
#define DRV_ACC_DEVTYPE_MPU6050 0x33
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+45 -11
View File
@@ -52,6 +52,8 @@ ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
isICM686 = config.custom2 == DRV_IMU_DEVTYPE_ICM42686P;
if (config.drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
@@ -125,7 +127,7 @@ int ICM42688P::probe()
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami == WHOAMI) {
if (whoami == WHOAMI || (isICM686 && whoami == WHOAMI686)) {
return PX4_OK;
} else {
@@ -160,7 +162,8 @@ void ICM42688P::RunImpl()
break;
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
if (((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI || (isICM686
&& RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI686)))
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
@@ -415,9 +418,17 @@ bool ICM42688P::Configure()
}
// 20-bits data format used
// the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
_px4_gyro.set_range(math::radians(2000.f));
// For the 688 the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
// For the 686 the only FSR settings that are operational are ±4000dps for gyroscope and ±32g for accelerometer
if (isICM686) {
_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
_px4_gyro.set_range(math::radians(4000.f));
} else {
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
_px4_gyro.set_range(math::radians(2000.f));
}
return success;
}
@@ -694,8 +705,14 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
}
if (!scale_20bit) {
// if highres enabled accel data is always 8192 LSB/g
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
// On the 686, if highres enabled accel data is always 4096 LSB/g
// On the 688, if highres enabled accel data is always 8192 LSB/g
if (isICM686) {
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
} else {
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
}
} else {
// 20 bit data scaled to 16 bit (2^4)
@@ -712,7 +729,12 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
accel.z[i] = accel_z;
}
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
if (isICM686) {
_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
} else {
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
}
}
// correct frame for publication
@@ -783,8 +805,14 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
}
if (!scale_20bit) {
// if highres enabled gyro data is always 131 LSB/dps
_px4_gyro.set_scale(math::radians(1.f / 131.f));
// On the 686, if highres enabled gyro data is always 65.5 LSB/dps
// On the 688, if highres enabled gyro data is always 131 LSB/dps
if (isICM686) {
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
} else {
_px4_gyro.set_scale(math::radians(1.f / 131.f));
}
} else {
// 20 bit data scaled to 16 bit (2^4)
@@ -794,7 +822,13 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
}
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
if (isICM686) {
_px4_gyro.set_scale(math::radians(2000.f / 16384.f));
} else {
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
}
}
// correct frame for publication
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+1
View File
@@ -219,4 +219,5 @@ private:
{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR },
{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR },
};
bool isICM686{false};
};
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
+1
View File
@@ -59,6 +59,7 @@ static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x47;
static constexpr uint8_t WHOAMI686 = 0x44;
static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
static constexpr float TEMPERATURE_OFFSET = 25.f; // C
src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
+8 -2
View File
@@ -44,6 +44,7 @@ void ICM42688P::print_usage()
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
PRINT_MODULE_USAGE_PARAM_FLAG('6', "Drive ICM-42686", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
@@ -54,7 +55,7 @@ extern "C" int icm42688p_main(int argc, char *argv[])
BusCLIArguments cli{false, true};
cli.default_spi_frequency = SPI_SPEED;
while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) {
while ((ch = cli.getOpt(argc, argv, "C:R:6")) != EOF) {
switch (ch) {
case 'C':
cli.custom1 = atoi(cli.optArg());
@@ -63,6 +64,10 @@ extern "C" int icm42688p_main(int argc, char *argv[])
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optArg());
break;
case '6':
cli.custom2 = DRV_IMU_DEVTYPE_ICM42686P;
break;
}
}
@@ -73,7 +78,8 @@ extern "C" int icm42688p_main(int argc, char *argv[])
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
BusInstanceIterator iterator(cli.custom2 == DRV_IMU_DEVTYPE_ICM42686P ? "icm42686p" : MODULE_NAME, cli,
cli.custom2 == DRV_IMU_DEVTYPE_ICM42686P ? DRV_IMU_DEVTYPE_ICM42686P : DRV_IMU_DEVTYPE_ICM42688P);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);