diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index eb21b01751..b1020c8eb4 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -149,7 +149,7 @@ const char * const lookupTableBaroHardware[] = {
#if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
// sync with magSensor_e
const char * const lookupTableMagHardware[] = {
- "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS3MDL"
+ "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS3MDL", "MAG_MPU925X_AK8963"
};
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
diff --git a/src/main/drivers/compass/compass_mpu925x_ak8963.c b/src/main/drivers/compass/compass_mpu925x_ak8963.c
new file mode 100644
index 0000000000..5ef5efdee1
--- /dev/null
+++ b/src/main/drivers/compass/compass_mpu925x_ak8963.c
@@ -0,0 +1,77 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/sensor.h"
+#include "drivers/system.h"
+#include "drivers/time.h"
+
+#include "drivers/accgyro/accgyro_mpu.h"
+
+#include "drivers/compass/compass.h"
+#include "drivers/compass/compass_ak8963.h"
+#include "drivers/compass/compass_mpu925x_ak8963.h"
+
+#if defined(USE_MAG_MPU925X_AK8963)
+
+#define MPU925X_I2C_ADDRESS 0x68
+#define AK8963_MAG_I2C_ADDRESS 0x0C
+#define MPU9250_BIT_RESET 0x80
+
+static bool mpu925xDeviceDetect(busDevice_t * dev)
+{
+ busWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
+ delay(150);
+ switch (busReadRegister(dev, MPU_RA_WHO_AM_I)) {
+ case MPU9250_WHO_AM_I_CONST:
+ case MPU9255_WHO_AM_I_CONST:
+ return true;
+ default:
+ return false;
+ }
+}
+
+bool mpu925Xak8963CompassDetect(magDev_t * mag)
+{
+ busDevice_t *busdev = &mag->busdev;
+ busdev->busdev_u.i2c.address = MPU925X_I2C_ADDRESS;
+ busDeviceRegister(busdev);
+ if (busdev == NULL || !mpu925xDeviceDetect(busdev)) {
+ return false;
+ }
+ // set bypass mode on mpu9250
+ busWriteRegister(busdev, MPU_RA_INT_PIN_CFG, 0x02);
+ delay(150);
+ // now we have ak8963 alike on the bus
+ busdev->busdev_u.i2c.address = AK8963_MAG_I2C_ADDRESS;
+ busDeviceRegister(busdev);
+ if(!ak8963Detect(mag)) {
+ // if ak8963 is not detected, reset the MPU to disable bypass mode
+ busdev->busdev_u.i2c.address = MPU925X_I2C_ADDRESS;
+ busWriteRegister(busdev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
+ return false;
+ } else {
+ return true;
+ }
+}
+
+#endif
diff --git a/src/main/drivers/compass/compass_mpu925x_ak8963.h b/src/main/drivers/compass/compass_mpu925x_ak8963.h
new file mode 100644
index 0000000000..91cffadc0a
--- /dev/null
+++ b/src/main/drivers/compass/compass_mpu925x_ak8963.h
@@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+
+#pragma once
+
+bool mpu925Xak8963CompassDetect(magDev_t *mag);
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 2592be4f1c..1b8f1c79dc 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -38,8 +38,10 @@
#include "drivers/compass/compass_ak8963.h"
#include "drivers/compass/compass_fake.h"
#include "drivers/compass/compass_hmc5883l.h"
-#include "drivers/compass/compass_qmc5883l.h"
#include "drivers/compass/compass_lis3mdl.h"
+#include "drivers/compass/compass_mpu925x_ak8963.h"
+#include "drivers/compass/compass_qmc5883l.h"
+
#include "drivers/io.h"
#include "drivers/light_led.h"
#include "drivers/time.h"
@@ -267,6 +269,18 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
break;
}
+ // MAG_MPU925X_AK8963 is an MPU925x configured as I2C passthrough to the built-in AK8963 magnetometer
+ // Passthrough mode disables the gyro/acc part of the MPU, so we only want to detect this sensor if mag_hardware was explicitly set to MAG_MPU925X_AK8963
+#ifdef USE_MAG_MPU925X_AK8963
+ if(compassConfig()->mag_hardware == MAG_MPU925X_AK8963){
+ if (mpu925Xak8963CompassDetect(dev)) {
+ magHardware = MAG_MPU925X_AK8963;
+ } else {
+ return false;
+ }
+ }
+#endif
+
if (magHardware == MAG_NONE) {
return false;
}
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 2be4553132..a74380072e 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -36,7 +36,8 @@ typedef enum {
MAG_AK8975 = 3,
MAG_AK8963 = 4,
MAG_QMC5883 = 5,
- MAG_LIS3MDL = 6
+ MAG_LIS3MDL = 6,
+ MAG_MPU925X_AK8963 = 7
} magSensor_e;
typedef struct mag_s {
diff --git a/src/main/target/STM32_UNIFIED/target.h b/src/main/target/STM32_UNIFIED/target.h
index 02d3dc69f7..00707e4dac 100644
--- a/src/main/target/STM32_UNIFIED/target.h
+++ b/src/main/target/STM32_UNIFIED/target.h
@@ -178,6 +178,7 @@
#define USE_MAG_QMC5883
#define USE_MAG_LIS3MDL
#define USE_MAG_AK8963
+#define USE_MAG_MPU925X_AK8963
#define USE_MAG_SPI_AK8963
#define USE_BARO