PX4 icm42688p:Add Support for the ICM42686

icm42688p:Fix colission on custom1 from rebase

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

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
+	//  For the 688 the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+	//  For the 686 the only FSR settings that are operational are ±4000dps for gyroscope and ±32g for accelerometer
-	_px4_gyro.set_range(math::radians(2000.f));
+
+	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
+		// On the 686, if highres enabled accel data is always 4096 LSB/g
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		// 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
+		// On the 686, if highres enabled gyro data is always 65.5 LSB/dps
-		_px4_gyro.set_scale(math::radians(1.f / 131.f));
+		// 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

@@ -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

@@ -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

@@ -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);