From 409c1bbf7a307c79a372d66da6d3041a8229fb9c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 5 Oct 2022 06:27:17 -0700 Subject: [PATCH] PX4 icm42688p:Add Support for the ICM42686 icm42688p:Fix colission on custom1 from rebase --- src/drivers/drv_sensor.h | 2 + .../imu/invensense/icm42688p/ICM42688P.cpp | 56 +++++++++++++++---- .../imu/invensense/icm42688p/ICM42688P.hpp | 1 + .../InvenSense_ICM42688P_registers.hpp | 1 + .../invensense/icm42688p/icm42688p_main.cpp | 10 +++- 5 files changed, 57 insertions(+), 13 deletions(-) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index d5033fd315..38f7c99c8b 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index df4178554f..b123768e0b 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -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 ×tamp_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 ×tamp_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 ×tamp_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 ×tamp_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 diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index 6eeb83e4a3..a04e74e707 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -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}; }; diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp index 21a6464092..6c407216f0 100644 --- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -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 diff --git a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp index 7a915b8325..0794f5cdef 100644 --- a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp +++ b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp @@ -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);