diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 00968141ee..54ba52249c 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -83,6 +83,8 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_fake.h
drivers/accgyro/accgyro_icm20689.c
drivers/accgyro/accgyro_icm20689.h
+ drivers/accgyro/accgyro_icm42605.c
+ drivers/accgyro/accgyro_icm42605.h
drivers/accgyro/accgyro_l3g4200d.c
drivers/accgyro/accgyro_l3g4200d.h
drivers/accgyro/accgyro_l3gd20.c
diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c
new file mode 100644
index 0000000000..5bf61e0760
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_icm42605.c
@@ -0,0 +1,272 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV. If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+#include "drivers/exti.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#include "drivers/accgyro/accgyro_icm42605.h"
+
+#if defined(USE_IMU_ICM42605)
+
+#define ICM42605_RA_PWR_MGMT0 0x4E
+
+#define ICM42605_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
+#define ICM42605_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
+#define ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
+#define ICM42605_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
+
+#define ICM42605_RA_GYRO_CONFIG0 0x4F
+#define ICM42605_RA_ACCEL_CONFIG0 0x50
+
+#define ICM42605_RA_GYRO_ACCEL_CONFIG0 0x52
+
+#define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
+#define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
+
+#define ICM42605_RA_GYRO_DATA_X1 0x25
+#define ICM42605_RA_ACCEL_DATA_X1 0x1F
+
+#define ICM42605_RA_INT_CONFIG 0x14
+#define ICM42605_INT1_MODE_PULSED (0 << 2)
+#define ICM42605_INT1_MODE_LATCHED (1 << 2)
+#define ICM42605_INT1_DRIVE_CIRCUIT_OD (0 << 1)
+#define ICM42605_INT1_DRIVE_CIRCUIT_PP (1 << 1)
+#define ICM42605_INT1_POLARITY_ACTIVE_LOW (0 << 0)
+#define ICM42605_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
+
+#define ICM42605_RA_INT_CONFIG0 0x63
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
+
+#define ICM42605_RA_INT_CONFIG1 0x64
+#define ICM42605_INT_ASYNC_RESET_BIT 4
+#define ICM42605_INT_TDEASSERT_DISABLE_BIT 5
+#define ICM42605_INT_TDEASSERT_ENABLED (0 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
+#define ICM42605_INT_TDEASSERT_DISABLED (1 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
+#define ICM42605_INT_TPULSE_DURATION_BIT 6
+#define ICM42605_INT_TPULSE_DURATION_100 (0 << ICM42605_INT_TPULSE_DURATION_BIT)
+#define ICM42605_INT_TPULSE_DURATION_8 (1 << ICM42605_INT_TPULSE_DURATION_BIT)
+
+
+#define ICM42605_RA_INT_SOURCE0 0x65
+#define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3)
+#define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3)
+
+
+static void icm42605AccInit(accDev_t *acc)
+{
+ acc->acc_1G = 512 * 4;
+}
+
+static bool icm42605AccRead(accDev_t *acc)
+{
+ uint8_t data[6];
+
+ const bool ack = busReadBuf(acc->busDev, ICM42605_RA_ACCEL_DATA_X1, data, 6);
+ if (!ack) {
+ return false;
+ }
+
+ acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
+ acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
+ acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
+
+ return true;
+}
+
+bool icm42605AccDetect(accDev_t *acc)
+{
+ acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM42605, acc->imuSensorToUse);
+ if (acc->busDev == NULL) {
+ return false;
+ }
+
+ mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+ if (ctx->chipMagicNumber != 0x4265) {
+ return false;
+ }
+
+ acc->initFn = icm42605AccInit;
+ acc->readFn = icm42605AccRead;
+ acc->accAlign = acc->busDev->param;
+
+ return true;
+}
+
+static const gyroFilterAndRateConfig_t icm42605GyroConfigs[] = {
+ /* DLPF ODR */
+ { GYRO_LPF_256HZ, 8000, { 6, 3 } }, /* 400 Hz LPF */
+ { GYRO_LPF_256HZ, 4000, { 5, 4 } }, /* 250 Hz LPF */
+ { GYRO_LPF_256HZ, 2000, { 3, 5 } }, /* 250 Hz LPF */
+ { GYRO_LPF_256HZ, 1000, { 1, 6 } }, /* 250 Hz LPF */
+ { GYRO_LPF_256HZ, 500, { 0, 15 } }, /* 250 Hz LPF */
+
+ { GYRO_LPF_188HZ, 1000, { 3, 6 } }, /* 125 HZ */
+ { GYRO_LPF_188HZ, 500, { 1, 15 } }, /* 125 HZ */
+
+ { GYRO_LPF_98HZ, 1000, { 4, 6 } }, /* 100 HZ*/
+ { GYRO_LPF_98HZ, 500, { 2, 15 } }, /* 100 HZ*/
+
+ { GYRO_LPF_42HZ, 1000, { 6, 6 } }, /* 50 HZ */
+ { GYRO_LPF_42HZ, 500, { 4, 15 } },
+
+ { GYRO_LPF_20HZ, 1000, { 7, 6 } }, /* 25 HZ */
+ { GYRO_LPF_20HZ, 500, { 6, 15 } },
+
+ { GYRO_LPF_10HZ, 1000, { 7, 6 } }, /* 25 HZ */
+ { GYRO_LPF_10HZ, 500, { 7, 15 } } /* 12.5 HZ */
+};
+
+static void icm42605AccAndGyroInit(gyroDev_t *gyro)
+{
+ busDevice_t * dev = gyro->busDev;
+ const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs,
+ &icm42605GyroConfigs[0], ARRAYLEN(icm42605GyroConfigs));
+ gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
+
+ gyroIntExtiInit(gyro);
+
+ busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
+
+ busWrite(dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
+ delay(15);
+
+ /* ODR and dynamic range */
+ busWrite(dev, ICM42605_RA_GYRO_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 2000 deg/s */
+ delay(15);
+
+ busWrite(dev, ICM42605_RA_ACCEL_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 16 G deg/s */
+ delay(15);
+
+ /* LPF bandwidth */
+ busWrite(dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, (config->gyroConfigValues[1]) | (config->gyroConfigValues[1] << 4));
+ delay(15);
+
+ busWrite(dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
+ delay(15);
+
+ busWrite(dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
+ delay(100);
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+ uint8_t intConfig1Value;
+
+ busWrite(dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
+
+ // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
+ busRead(dev, ICM42605_RA_INT_CONFIG1, &intConfig1Value);
+
+ intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT);
+ intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED);
+
+ busWrite(dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
+ delay(15);
+#endif
+
+ busSetSpeed(dev, BUS_SPEED_FAST);
+}
+
+static bool icm42605DeviceDetect(busDevice_t * dev)
+{
+ uint8_t tmp;
+ uint8_t attemptsRemaining = 5;
+
+ busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
+
+ busWrite(dev, ICM42605_RA_PWR_MGMT0, 0x00);
+
+ do {
+ delay(150);
+
+ busRead(dev, MPU_RA_WHO_AM_I, &tmp);
+
+ switch (tmp) {
+ /* ICM42605 and ICM42688P share the register structure*/
+ case ICM42605_WHO_AM_I_CONST:
+ case ICM42688P_WHO_AM_I_CONST:
+ return true;
+
+ default:
+ // Retry detection
+ break;
+ }
+ } while (attemptsRemaining--);
+
+ return false;
+}
+
+static bool icm42605GyroRead(gyroDev_t *gyro)
+{
+ uint8_t data[6];
+
+ const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_GYRO_DATA_X1, data, 6);
+ if (!ack) {
+ return false;
+ }
+
+ gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
+ gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
+ gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
+
+ return true;
+}
+
+bool icm42605GyroDetect(gyroDev_t *gyro)
+{
+ gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
+ if (gyro->busDev == NULL) {
+ return false;
+ }
+
+ if (!icm42605DeviceDetect(gyro->busDev)) {
+ busDeviceDeInit(gyro->busDev);
+ return false;
+ }
+
+ // Magic number for ACC detection to indicate that we have detected icm42605 gyro
+ mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+ ctx->chipMagicNumber = 0x4265;
+
+ gyro->initFn = icm42605AccAndGyroInit;
+ gyro->readFn = icm42605GyroRead;
+ gyro->intStatusFn = gyroCheckDataReady;
+ gyro->temperatureFn = NULL;
+ gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
+ gyro->gyroAlign = gyro->busDev->param;
+
+ return true;
+}
+
+#endif
diff --git a/src/main/drivers/accgyro/accgyro_icm42605.h b/src/main/drivers/accgyro/accgyro_icm42605.h
new file mode 100644
index 0000000000..5224222f26
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_icm42605.h
@@ -0,0 +1,21 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV. If not, see .
+ */
+
+#pragma once
+
+bool icm42605AccDetect(accDev_t *acc);
+bool icm42605GyroDetect(gyroDev_t *gyro);
diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index b6b8cb38f3..16d7f5ac19 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -35,6 +35,8 @@
#define ICM20602_WHO_AM_I_CONST (0x12)
#define ICM20608G_WHO_AM_I_CONST (0xAF)
#define ICM20689_WHO_AM_I_CONST (0x98)
+#define ICM42605_WHO_AM_I_CONST (0x42)
+#define ICM42688P_WHO_AM_I_CONST (0x47)
// RA = Register Address
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index eefa81eb10..7b9282ae99 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -94,6 +94,7 @@ typedef enum {
DEVHW_BMI088_GYRO,
DEVHW_BMI088_ACC,
DEVHW_ICM20689,
+ DEVHW_ICM42605,
/* Combined ACC/GYRO/MAG chips */
DEVHW_MPU9250,
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index ef745dc858..991dd5d2b4 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -164,7 +164,7 @@ static const char * const featureNames[] = {
/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
// sync with gyroSensor_e
-static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"};
+static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "FAKE"};
// sync this with sensors_e
static const char * const sensorTypeNames[] = {
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 1d821e4467..cc8b097399 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4,7 +4,7 @@ tables:
- name: gyro_lpf
values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"]
- name: acc_hardware
- values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"]
+ values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "FAKE"]
enum: accelerationSensor_e
- name: rangefinder_hardware
values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"]
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 1f5d1de04d..d795b05bcb 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -53,6 +53,7 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/accgyro/accgyro_bmi088.h"
#include "drivers/accgyro/accgyro_bmi160.h"
#include "drivers/accgyro/accgyro_icm20689.h"
+#include "drivers/accgyro/accgyro_icm42605.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/sensor.h"
@@ -268,6 +269,19 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
FALLTHROUGH;
#endif
+#ifdef USE_IMU_ICM42605
+ case ACC_ICM42605:
+ if (icm42605AccDetect(dev)) {
+ accHardware = ACC_ICM42605;
+ break;
+ }
+ /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+ if (accHardwareToUse != ACC_AUTODETECT) {
+ break;
+ }
+ FALLTHROUGH;
+#endif
+
#ifdef USE_IMU_FAKE
case ACC_FAKE:
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index a650b4be95..08b404dccb 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -33,20 +33,21 @@
// Type of accelerometer used/detected
typedef enum {
- ACC_NONE = 0,
- ACC_AUTODETECT = 1,
- ACC_ADXL345 = 2,
- ACC_MPU6050 = 3,
- ACC_MMA8452 = 4,
- ACC_BMA280 = 5,
- ACC_LSM303DLHC = 6,
- ACC_MPU6000 = 7,
- ACC_MPU6500 = 8,
- ACC_MPU9250 = 9,
- ACC_BMI160 = 10,
- ACC_ICM20689 = 11,
- ACC_BMI088 = 12,
- ACC_FAKE = 13,
+ ACC_NONE = 0,
+ ACC_AUTODETECT = 1,
+ ACC_ADXL345 = 2,
+ ACC_MPU6050 = 3,
+ ACC_MMA8452 = 4,
+ ACC_BMA280 = 5,
+ ACC_LSM303DLHC = 6,
+ ACC_MPU6000 = 7,
+ ACC_MPU6500 = 8,
+ ACC_MPU9250 = 9,
+ ACC_BMI160 = 10,
+ ACC_ICM20689 = 11,
+ ACC_BMI088 = 12,
+ ACC_ICM42605 = 13,
+ ACC_FAKE,
ACC_MAX = ACC_FAKE
} accelerationSensor_e;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index f70bdbcdf0..d605a4528a 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -55,6 +55,7 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/accgyro/accgyro_bmi088.h"
#include "drivers/accgyro/accgyro_bmi160.h"
#include "drivers/accgyro/accgyro_icm20689.h"
+#include "drivers/accgyro/accgyro_icm42605.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/io.h"
@@ -247,6 +248,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
FALLTHROUGH;
#endif
+#ifdef USE_IMU_ICM42605
+ case GYRO_ICM42605:
+ if (icm42605GyroDetect(dev)) {
+ gyroHardware = GYRO_ICM42605;
+ break;
+ }
+ FALLTHROUGH;
+#endif
+
#ifdef USE_IMU_FAKE
case GYRO_FAKE:
if (fakeGyroDetect(dev)) {
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index eed2aceb60..3957769e0b 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -37,6 +37,7 @@ typedef enum {
GYRO_BMI160,
GYRO_ICM20689,
GYRO_BMI088,
+ GYRO_ICM42605,
GYRO_FAKE
} gyroSensor_e;
diff --git a/src/main/target/MATEKF765/CMakeLists.txt b/src/main/target/MATEKF765/CMakeLists.txt
index 2461647ec0..0efbb358d6 100644
--- a/src/main/target/MATEKF765/CMakeLists.txt
+++ b/src/main/target/MATEKF765/CMakeLists.txt
@@ -1 +1,2 @@
target_stm32f765xi(MATEKF765)
+target_stm32f765xi(MATEKF765SE)
diff --git a/src/main/target/MATEKF765/target.c b/src/main/target/MATEKF765/target.c
index af157cad89..a1d8c151fa 100644
--- a/src/main/target/MATEKF765/target.c
+++ b/src/main/target/MATEKF765/target.c
@@ -29,6 +29,10 @@
BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN);
+#if defined(MATEKF765SE)
+BUSDEV_REGISTER_SPI_TAG(busdev_imu3, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
+#endif
+
const timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,7), D(1,5,3)
DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 UP(1,7), D(1,6,3)
diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h
index bb2d33d094..391e95e9e6 100644
--- a/src/main/target/MATEKF765/target.h
+++ b/src/main/target/MATEKF765/target.h
@@ -56,6 +56,13 @@
#define MPU6500_CS_PIN PD7
#define MPU6500_EXTI_PIN PD4
+#if defined(MATEKF765SE)
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN CW0_DEG_FLIP
+#define ICM42605_SPI_BUS BUS_SPI4
+#define ICM42605_CS_PIN PE11
+#define ICM42605_EXTI_PIN PC13
+#endif
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
@@ -103,12 +110,10 @@
#define MAX7456_CS_PIN PB12
// *************** SPI4 ******************************
-/*
#define USE_SPI_DEVICE_4
#define SPI4_SCK_PIN PE12
#define SPI4_MISO_PIN PE13
#define SPI4_MOSI_PIN PE14
-*/
// *************** UART *****************************
#define USE_VCP
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index bc32325ced..d9b803e143 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -73,6 +73,10 @@
BUSDEV_REGISTER_SPI(busdev_icm20689, DEVHW_ICM20689, ICM20689_SPI_BUS, ICM20689_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_ICM20689_ALIGN);
#endif
+ #if defined(USE_IMU_ICM42605)
+ BUSDEV_REGISTER_SPI(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
+ #endif
+
#if defined(USE_IMU_BMI160)
#if defined(BMI160_SPI_BUS)
BUSDEV_REGISTER_SPI(busdev_bmi160, DEVHW_BMI160, BMI160_SPI_BUS, BMI160_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI160_ALIGN);