diff --git a/src/configuration.h b/src/configuration.h
index a31198346d7..2c153f3d43c 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -292,6 +292,9 @@ along with this program. If not, see .
#define DA217_ADDR 0x26
#define BMI270_ADDR 0x68
#define BMI270_ADDR_ALT 0x69
+#define MPU9250_ADDR 0x68
+#define MPU9250_ADDR_ALT 0x69
+#define AK8963_ADDR 0x0C
// -----------------------------------------------------------------------------
// LED
diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp
index 75eabc9541d..2d11b346cf9 100644
--- a/src/detect/ScanI2C.cpp
+++ b/src/detect/ScanI2C.cpp
@@ -37,8 +37,9 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
{
- ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948, QMA6100P, BMM150, BMI270};
- return firstOfOrNONE(10, types);
+ ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX,
+ ICM20948, QMA6100P, BMM150, BMI270, MPU9250};
+ return firstOfOrNONE(11, types);
}
ScanI2C::FoundDevice ScanI2C::firstAQI() const
diff --git a/src/detect/ScanI2C.h b/src/detect/ScanI2C.h
index 054c7854baf..775c44bc28c 100644
--- a/src/detect/ScanI2C.h
+++ b/src/detect/ScanI2C.h
@@ -92,6 +92,7 @@ class ScanI2C
CST226SE,
CST3530,
BMI270,
+ MPU9250,
SEN5X,
SFA30,
CW2015,
diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index c1c48eac88b..070e065748d 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -796,11 +796,19 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
type = BMX160;
logFoundDevice("BMX160", (uint8_t)addr.address);
break;
- } else {
- type = MPU6050;
- logFoundDevice("MPU6050", (uint8_t)addr.address);
+ }
+ // MPU-6050 and MPU-9250/9255 share I2C addresses (0x68/0x69), and chip ID
+ // register 0x00 reads 0x00 on MPU-9250. Disambiguate via WHO_AM_I (0x75):
+ // MPU-6050 -> 0x68, MPU-9250 -> 0x71, MPU-9255 -> 0x73
+ registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x75), 1);
+ if (registerValue == 0x71 || registerValue == 0x73) {
+ type = MPU9250;
+ logFoundDevice(registerValue == 0x73 ? "MPU9255" : "MPU9250", (uint8_t)addr.address);
break;
}
+ type = MPU6050;
+ logFoundDevice("MPU6050", (uint8_t)addr.address);
+ break;
}
break;
diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h
index d2205fd2a3e..bb785de2325 100755
--- a/src/motion/AccelerometerThread.h
+++ b/src/motion/AccelerometerThread.h
@@ -19,6 +19,7 @@
#include "LIS3DHSensor.h"
#include "LSM6DS3Sensor.h"
#include "MPU6050Sensor.h"
+#include "MPU9250Sensor.h"
#include "MotionSensor.h"
#ifdef HAS_QMA6100P
#include "QMA6100PSensor.h"
@@ -111,6 +112,9 @@ class AccelerometerThread : public concurrency::OSThread
case ScanI2C::DeviceType::ICM20948:
sensor = new ICM20948Sensor(device);
break;
+ case ScanI2C::DeviceType::MPU9250:
+ sensor = new MPU9250Sensor(device);
+ break;
case ScanI2C::DeviceType::BMM150:
sensor = new BMM150Sensor(device);
break;
diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
new file mode 100644
index 00000000000..2d9bc2742b3
--- /dev/null
+++ b/src/motion/MPU9250Sensor.cpp
@@ -0,0 +1,323 @@
+#include "MPU9250Sensor.h"
+
+#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
+
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
+// screen is defined in main.cpp
+extern graphics::Screen *screen;
+#endif
+
+namespace
+{
+// MPU-6500 die registers
+constexpr uint8_t MPU_SMPLRT_DIV = 0x19;
+constexpr uint8_t MPU_CONFIG = 0x1A;
+constexpr uint8_t MPU_GYRO_CONFIG = 0x1B;
+constexpr uint8_t MPU_ACCEL_CONFIG = 0x1C;
+constexpr uint8_t MPU_INT_PIN_CFG = 0x37;
+constexpr uint8_t MPU_ACCEL_XOUT_H = 0x3B;
+constexpr uint8_t MPU_USER_CTRL = 0x6A;
+constexpr uint8_t MPU_PWR_MGMT_1 = 0x6B;
+constexpr uint8_t MPU_WHO_AM_I = 0x75;
+
+// AK8963 magnetometer die registers
+constexpr uint8_t AK_WIA = 0x00;
+constexpr uint8_t AK_ST1 = 0x02;
+constexpr uint8_t AK_HXL = 0x03;
+constexpr uint8_t AK_ST2 = 0x09;
+constexpr uint8_t AK_CNTL1 = 0x0A;
+constexpr uint8_t AK_CNTL2 = 0x0B;
+constexpr uint8_t AK_ASAX = 0x10;
+
+// AK8963 control values
+constexpr uint8_t AK_MODE_POWERDOWN = 0x00;
+constexpr uint8_t AK_MODE_FUSE_ROM = 0x0F;
+constexpr uint8_t AK_MODE_CONT2_16BIT = 0x16; // continuous mode 2 (100 Hz), 16-bit output
+
+// Conversions
+// ±2g full scale, 16-bit signed -> g per LSB
+constexpr float ACCEL_LSB_PER_G = 16384.0f;
+// AK8963 16-bit output: 0.15 µT per LSB (datasheet 6.3)
+constexpr float MAG_UT_PER_LSB = 0.15f;
+} // namespace
+
+MPU9250Sensor::MPU9250Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
+
+TwoWire *MPU9250Sensor::resolveBus() const
+{
+#if defined(WIRE_INTERFACES_COUNT) && (WIRE_INTERFACES_COUNT > 1)
+ return device.address.port == ScanI2C::I2CPort::WIRE1 ? &Wire1 : &Wire;
+#else
+ return &Wire;
+#endif
+}
+
+bool MPU9250Sensor::writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value)
+{
+ bus->beginTransmission(i2cAddr);
+ bus->write(reg);
+ bus->write(value);
+ return bus->endTransmission() == 0;
+}
+
+bool MPU9250Sensor::readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, uint8_t len)
+{
+ bus->beginTransmission(i2cAddr);
+ bus->write(reg);
+ if (bus->endTransmission(false) != 0) {
+ return false;
+ }
+ const uint8_t received = bus->requestFrom((int)i2cAddr, (int)len);
+ if (received != len) {
+ return false;
+ }
+ for (uint8_t i = 0; i < len; ++i) {
+ buf[i] = bus->read();
+ }
+ return true;
+}
+
+bool MPU9250Sensor::initMPU6500()
+{
+ const uint8_t addr = deviceAddress();
+
+ // Sanity-check WHO_AM_I — scan should have already verified this but be defensive
+ uint8_t whoAmI = 0;
+ if (!readRegisters(addr, MPU_WHO_AM_I, &whoAmI, 1)) {
+ LOG_DEBUG("MPU9250 WHO_AM_I read failed at 0x%02X", addr);
+ return false;
+ }
+ if (whoAmI != 0x71 && whoAmI != 0x73) {
+ LOG_DEBUG("MPU9250 unexpected WHO_AM_I 0x%02X at 0x%02X", whoAmI, addr);
+ return false;
+ }
+
+ // Reset device
+ if (!writeRegister(addr, MPU_PWR_MGMT_1, 0x80)) {
+ return false;
+ }
+ delay(100);
+
+ // Wake up, select PLL with X gyro reference (better stability than internal osc)
+ if (!writeRegister(addr, MPU_PWR_MGMT_1, 0x01)) {
+ return false;
+ }
+ delay(50);
+
+ // DLPF 41 Hz (CONFIG = 3), sample rate 1 kHz / (1 + SMPLRT_DIV) = 200 Hz
+ writeRegister(addr, MPU_CONFIG, 0x03);
+ writeRegister(addr, MPU_SMPLRT_DIV, 0x04);
+
+ // ±2 g accel range
+ writeRegister(addr, MPU_ACCEL_CONFIG, 0x00);
+ // ±250 dps gyro (gyro is unused for compass but configuring leaves the chip in a known state)
+ writeRegister(addr, MPU_GYRO_CONFIG, 0x00);
+
+ // Expose AK8963 on the main I2C bus: disable internal master, enable bypass
+ writeRegister(addr, MPU_USER_CTRL, 0x00);
+ writeRegister(addr, MPU_INT_PIN_CFG, 0x02);
+ delay(10);
+
+ return true;
+}
+
+bool MPU9250Sensor::initAK8963()
+{
+ uint8_t wia = 0;
+ if (!readRegisters(AK8963_ADDR, AK_WIA, &wia, 1) || wia != 0x48) {
+ LOG_DEBUG("MPU9250 AK8963 WHO_AM_I unexpected (0x%02X)", wia);
+ return false;
+ }
+
+ // Soft reset
+ writeRegister(AK8963_ADDR, AK_CNTL2, 0x01);
+ delay(100);
+
+ // Power-down then enter Fuse ROM access to read per-axis sensitivity (ASA)
+ writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN);
+ delay(10);
+ writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_FUSE_ROM);
+ delay(10);
+
+ uint8_t asa[3] = {0};
+ if (!readRegisters(AK8963_ADDR, AK_ASAX, asa, 3)) {
+ LOG_DEBUG("MPU9250 AK8963 ASA read failed");
+ return false;
+ }
+ // Sensitivity adjustment: H_adj = H * ((ASA - 128) * 0.5 / 128 + 1) — datasheet 8.3.11
+ for (int i = 0; i < 3; ++i) {
+ asaScale[i] = (((float)asa[i] - 128.0f) * 0.5f / 128.0f) + 1.0f;
+ }
+
+ // Back to power-down, then enter continuous-mode-2 (100 Hz) at 16-bit resolution
+ writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN);
+ delay(10);
+ writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_CONT2_16BIT);
+ delay(10);
+
+ return true;
+}
+
+bool MPU9250Sensor::init()
+{
+ bus = resolveBus();
+ if (!bus) {
+ return false;
+ }
+
+ if (!initMPU6500()) {
+ LOG_DEBUG("MPU9250 MPU-6500 init failed");
+ return false;
+ }
+ if (!initAK8963()) {
+ LOG_DEBUG("MPU9250 AK8963 init failed");
+ return false;
+ }
+
+ loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+ LOG_DEBUG("MPU9250 init ok (asa=%.2f,%.2f,%.2f)", asaScale[0], asaScale[1], asaScale[2]);
+ return true;
+}
+
+bool MPU9250Sensor::readSensors(FusionVector &accel, FusionVector &mag)
+{
+ // Read 6 bytes of accel data
+ uint8_t raw[6] = {0};
+ if (!readRegisters(deviceAddress(), MPU_ACCEL_XOUT_H, raw, 6)) {
+ return false;
+ }
+ const int16_t ax = (int16_t)((raw[0] << 8) | raw[1]);
+ const int16_t ay = (int16_t)((raw[2] << 8) | raw[3]);
+ const int16_t az = (int16_t)((raw[4] << 8) | raw[5]);
+ accel.axis.x = (float)ax / ACCEL_LSB_PER_G;
+ accel.axis.y = (float)ay / ACCEL_LSB_PER_G;
+ accel.axis.z = (float)az / ACCEL_LSB_PER_G;
+
+ // Check magnetometer data-ready bit
+ uint8_t st1 = 0;
+ if (!readRegisters(AK8963_ADDR, AK_ST1, &st1, 1) || (st1 & 0x01) == 0) {
+ return false;
+ }
+
+ // Read 6 bytes of mag (little-endian) plus ST2 to release the data register.
+ // ST2 also exposes the magnetic overflow flag (bit 3) — discard the sample if set.
+ uint8_t magRaw[7] = {0};
+ if (!readRegisters(AK8963_ADDR, AK_HXL, magRaw, 7)) {
+ return false;
+ }
+ if (magRaw[6] & 0x08) {
+ return false;
+ }
+ const int16_t mx = (int16_t)((magRaw[1] << 8) | magRaw[0]);
+ const int16_t my = (int16_t)((magRaw[3] << 8) | magRaw[2]);
+ const int16_t mz = (int16_t)((magRaw[5] << 8) | magRaw[4]);
+
+ mag.axis.x = (float)mx * MAG_UT_PER_LSB * asaScale[0];
+ mag.axis.y = (float)my * MAG_UT_PER_LSB * asaScale[1];
+ mag.axis.z = (float)mz * MAG_UT_PER_LSB * asaScale[2];
+ return true;
+}
+
+int32_t MPU9250Sensor::runOnce()
+{
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+ FusionVector accel = {{0, 0, 0}};
+ FusionVector mag = {{0, 0, 0}};
+ if (!readSensors(accel, mag)) {
+ return MOTION_SENSOR_CHECK_INTERVAL_MS;
+ }
+
+ if (doCalibration) {
+ beginCalibrationDisplay(showingScreen);
+ updateCalibrationExtrema(mag.axis.x, mag.axis.y, mag.axis.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+ finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
+ lowestZ);
+ }
+
+ // Subtract hard-iron offsets
+ mag.axis.x -= (highestX + lowestX) / 2;
+ mag.axis.y -= (highestY + lowestY) / 2;
+ mag.axis.z -= (highestZ + lowestZ) / 2;
+
+ // Smooth raw inputs with a per-axis EMA to suppress dynamic acceleration
+ // noise during device rotation.
+ if (!filtersSeeded) {
+ accelFiltered = accel;
+ magFiltered = mag;
+ filtersSeeded = true;
+ } else {
+ for (int i = 0; i < 3; ++i) {
+ accelFiltered.array[i] = accelFilterAlpha * accel.array[i] + (1.0f - accelFilterAlpha) * accelFiltered.array[i];
+ magFiltered.array[i] = magFilterAlpha * mag.array[i] + (1.0f - magFilterAlpha) * magFiltered.array[i];
+ }
+ }
+ accel = accelFiltered;
+ mag = magFiltered;
+
+ // The MPU-6500 accelerometer and AK8963 magnetometer dies sit inside the
+ // same package but use different axis conventions (datasheet 7.4 vs 8.1).
+ // To express the mag reading in the accel frame we swap X<->Y and negate Z.
+ // Final tweaks for board orientation are handled by config.display.compass_orientation.
+ FusionVector ga, ma;
+ ga.axis.x = accel.axis.x;
+ ga.axis.y = -accel.axis.y;
+ ga.axis.z = -accel.axis.z;
+ ma.axis.x = mag.axis.y;
+ ma.axis.y = mag.axis.x;
+ ma.axis.z = -mag.axis.z;
+
+ // Compensate for non-flat case mounting. FusionCompassCalculateHeading()
+ // assumes Z is the up axis — when the baseboard is mounted vertically
+ // (e.g. a case where the LCD sits perpendicular to the board), chip Z is
+ // horizontal and the tilt-comp math becomes unstable. Override which chip
+ // axis is treated as world-up via the MPU9250_UP_AXIS_* defines.
+ // _PZ (default) — chip +Z up (baseboard flat)
+ // _PX — chip +X up (vertical mount, silkscreen "north" up)
+ // _NX — chip -X up
+ // _PY — chip +Y up
+ // _NY — chip -Y up
+#ifndef MPU9250_UP_AXIS
+#define MPU9250_UP_AXIS MPU9250_UP_AXIS_PZ
+#endif
+#if MPU9250_UP_AXIS == MPU9250_UP_AXIS_PX
+ ga = FusionAxesSwap(ga, FusionAxesAlignmentNZPYPX);
+ ma = FusionAxesSwap(ma, FusionAxesAlignmentNZPYPX);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NX
+ ga = FusionAxesSwap(ga, FusionAxesAlignmentPZPYNX);
+ ma = FusionAxesSwap(ma, FusionAxesAlignmentPZPYNX);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_PY
+ ga = FusionAxesSwap(ga, FusionAxesAlignmentPXNZPY);
+ ma = FusionAxesSwap(ma, FusionAxesAlignmentPXNZPY);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NY
+ ga = FusionAxesSwap(ga, FusionAxesAlignmentPXPZNY);
+ ma = FusionAxesSwap(ma, FusionAxesAlignmentPXPZNY);
+#endif
+
+ if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
+ ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+ ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+ }
+
+ float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+ heading = applyCompassOrientation(heading);
+ if (screen)
+ screen->setHeading(heading);
+#endif
+ return MOTION_SENSOR_CHECK_INTERVAL_MS;
+}
+
+void MPU9250Sensor::calibrate(uint16_t forSeconds)
+{
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+ LOG_DEBUG("MPU9250 calibration started for %is", forSeconds);
+ FusionVector accel, mag;
+ if (readSensors(accel, mag)) {
+ seedCalibrationExtrema(mag.axis.x, mag.axis.y, mag.axis.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+ } else {
+ seedCalibrationExtrema(0.0f, 0.0f, 0.0f, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+ }
+ startCalibrationWindow(forSeconds);
+#endif
+}
+
+#endif
diff --git a/src/motion/MPU9250Sensor.h b/src/motion/MPU9250Sensor.h
new file mode 100644
index 00000000000..4e789892476
--- /dev/null
+++ b/src/motion/MPU9250Sensor.h
@@ -0,0 +1,75 @@
+#pragma once
+#ifndef _MPU9250_SENSOR_H_
+#define _MPU9250_SENSOR_H_
+
+#include "MotionSensor.h"
+
+#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
+
+#include "Fusion/Fusion.h"
+#include
+
+// Numeric IDs for selecting which chip axis maps to world-up.
+// Set MPU9250_UP_AXIS to one of these in variant.h or via build flags.
+#define MPU9250_UP_AXIS_PZ 0
+#define MPU9250_UP_AXIS_PX 1
+#define MPU9250_UP_AXIS_NX 2
+#define MPU9250_UP_AXIS_PY 3
+#define MPU9250_UP_AXIS_NY 4
+
+// InvenSense MPU-9250 (and MPU-9255) 9-axis IMU driver.
+// The package contains an MPU-6500 accel/gyro die plus an AK8963 magnetometer
+// die. We drive both directly over I2C: the MPU-6500 at deviceAddress() and the
+// AK8963 at AK8963_ADDR via the MPU's I2C bypass mode.
+// This driver covers the RAK1905 WisBlock module (MPU-9250) — a viable
+// replacement for the EOL RAK12034 (BMX160) when tilt-compensated heading is
+// needed alongside RAK1904 (LIS3DH) options.
+class MPU9250Sensor : public MotionSensor
+{
+ private:
+ TwoWire *bus = nullptr;
+ bool showingScreen = false;
+
+ // Per-axis AK8963 factory sensitivity adjustment scale factors derived from
+ // the chip's Fuse ROM (ASA registers). Applied to every raw mag sample.
+ float asaScale[3] = {1.0f, 1.0f, 1.0f};
+
+ // Hard-iron calibration extrema persisted to flash.
+ static constexpr const char *compassCalibrationFileName = "/prefs/compass_mpu9250.dat";
+ float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
+
+ // Exponential moving average on raw accel + mag to keep the heading stable
+ // during device rotation. The compass-only fusion path is stateless, so any
+ // dynamic acceleration shows up immediately as heading noise; filtering the
+ // inputs is the cheapest mitigation.
+ static constexpr float accelFilterAlpha = 0.15f;
+ static constexpr float magFilterAlpha = 0.20f;
+ FusionVector accelFiltered = {{0, 0, 0}};
+ FusionVector magFiltered = {{0, 0, 0}};
+ bool filtersSeeded = false;
+
+ // Pick the correct TwoWire instance for the detected device.
+ TwoWire *resolveBus() const;
+
+ // Low-level I2C helpers — one address per call so we can address both dies.
+ bool writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value);
+ bool readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, uint8_t len);
+
+ // Init helpers.
+ bool initMPU6500();
+ bool initAK8963();
+
+ // Read accel (g) and mag (µT) into Fusion vectors; returns false if mag
+ // had no fresh sample on this tick.
+ bool readSensors(FusionVector &accel, FusionVector &mag);
+
+ public:
+ explicit MPU9250Sensor(ScanI2C::FoundDevice foundDevice);
+ virtual bool init() override;
+ virtual int32_t runOnce() override;
+ virtual void calibrate(uint16_t forSeconds) override;
+};
+
+#endif
+
+#endif